Rev Author Line No. Line
799 cizelu 1 #include "main.h"
842 cizelu 2  
708 cizelu 3 // NEPOUZIVAT PINY B6 A B7, JSOU VYHRAZENY PRO SERIOVOU KOMUNIKACI
842 cizelu 4 // BAUD RATE = 9600
5  
6 // univerzalni LED diody
830 cizelu 7 #define LED1 PIN_E0
8 #define LED2 PIN_E1
700 cizelu 9  
842 cizelu 10 // piezo pipak
716 cizelu 11 #DEFINE SOUND_HI PIN_B4
12 #DEFINE SOUND_LO PIN_B5
700 cizelu 13  
842 cizelu 14 // radkovy senzor
745 cizelu 15 #define SDIN PIN_D4 // seriovy vstup
830 cizelu 16 #define SDOUT input(PIN_C5) // seriovy vystup
745 cizelu 17 #define SCLK PIN_D5 // takt
727 cizelu 18  
842 cizelu 19 #define OLSA_LEV 10 // rozhodovaci uroven (cca 10 odpovida cerne)
840 cizelu 20  
842 cizelu 21 // pro komunikaci s OLSA, prvni se posila LSB
829 cizelu 22 int main_reset[8]={1,1,0,1,1,0,0,0}; // hlavni reset 0x1B
23 int set_mode_rg[8]={1,1,1,1,1,0,1,0}; // zapis do MODE registru 0x5F
24 int clear_mode_rg[8]={0,0,0,0,0,0,0,0}; // nulovani MODE registru 0x00
730 cizelu 25  
829 cizelu 26 int left_offset[8]={0,0,0,0,0,0,1,0}; // offset leveho segmentu senzoru 0x40
27 int mid_offset[8]={0,1,0,0,0,0,1,0}; // offset prostredniho segmentu senzoru 0x42
28 int right_offset[8]={0,0,1,0,0,0,1,0}; // offset praveho segmentu senzoru 0x44
29 int offset[8]={1,0,0,0,0,0,0,1}; // minus jedna - pouzit pro vsechny segmenty 0x81
741 cizelu 30  
829 cizelu 31 int left_gain[8]={1,0,0,0,0,0,1,0}; // zisk leveho segmentu 0x41
32 int mid_gain[8]={1,1,0,0,0,0,1,0}; // zisk leveho segmentu 0x43
33 int right_gain[8]={1,0,1,0,0,0,1,0}; // zisk leveho segmentu 0x45
34 int gain[8]={1,0,1,0,0,0,0,0}; // zisk = 5 - pouzit pro vsechny segmenty 0x5
771 kaklik 35  
829 cizelu 36 int start_int[8]={0,0,0,1,0,0,0,0}; // zacatek integrace 0x08
37 int stop_int[8]={0,0,0,0,1,0,0,0}; // konec integrace 0x10
38 int readout[8]={0,1,0,0,0,0,0,0}; // cteni senzoru 0x02
789 cizelu 39  
829 cizelu 40 int olsa_lseg[51]={0}; // leva cast radky (pixely 0 - 50)
41 int olsa_rseg[51]={0}; // prava cast radky (pixely 51 - 101)
837 cizelu 42 int8 *lp; // ukazatel pro levou polovinu radky
43 int8 *rp; // ukazatel pro levou polovinu radky
812 cizelu 44  
842 cizelu 45 //naraznik
708 cizelu 46 #define BUMPL input(PIN_D6)
47 #define BUMPR input(PIN_D7)
700 cizelu 48  
842 cizelu 49 //nouzove senzory
708 cizelu 50 #define LINEL 0
51 #define LINER 1
842 cizelu 52 #define BLUE_LEV 200 // rozhodovaci uroven (zmereno 0 - cerna, 255 cerna)
789 cizelu 53 int8 line_l;
54 int8 line_r;
55 int1 line_position;
700 cizelu 56  
842 cizelu 57 // motory
708 cizelu 58 #define LMF PIN_D0
59 #define LMB PIN_D1
60 #define RMF PIN_D2
61 #define RMB PIN_D3
842 cizelu 62  
63 int8 lm_speed;
790 cizelu 64 int8 rm_speed;
65  
700 cizelu 66 //PODPROGRAMY
67 //SENZORY
730 cizelu 68 //OLSA01A
842 cizelu 69 void olsa_pulses(int count) // vytvori impulzy pro ridici logiku
829 cizelu 70 {
741 cizelu 71 int8 ct;
72 for(ct=0;ct<=count;ct++)
829 cizelu 73 {
730 cizelu 74 output_high(SCLK);
75 output_low(SCLK);
829 cizelu 76 }
77 }
730 cizelu 78  
842 cizelu 79 void olsa_pulse() // vytvori jeden impulz
829 cizelu 80 {
741 cizelu 81 output_high(SCLK);
82 output_low(SCLK);
829 cizelu 83 }
741 cizelu 84  
842 cizelu 85 void olsa_send(int8 info[8]) // USART komunikace s modulem OLSA01A - poslani zpravy
829 cizelu 86 {
842 cizelu 87 int *ip; // ukazatel na pole s informaci
88 int8 i; // pomocna promenna pro nastaveni 0 nebo 1 na SDIN
89 output_low(SDIN); // start bit
741 cizelu 90 olsa_pulse();
842 cizelu 91 for(ip=0;ip<8;ip++) // predani informace - 8 bit, LSB prvni > MSB posledni
829 cizelu 92 {
842 cizelu 93 i=info[ip]; // ziskani hodnoty z pole
94 if(i==1) // vyhodnoceni obsahu informace - nastav 1
829 cizelu 95 {
730 cizelu 96 output_high(SDIN);
829 cizelu 97 }
842 cizelu 98 else // vyhodnoceni obsahu informace - nastav 0
829 cizelu 99 {
730 cizelu 100 output_low(SDIN);
829 cizelu 101 }
741 cizelu 102 olsa_pulse();
829 cizelu 103 }
842 cizelu 104 output_high(SDIN); // stop bit
741 cizelu 105 olsa_pulse();
829 cizelu 106 }
730 cizelu 107  
842 cizelu 108 void olsa_reset() // hlavni RESET - provadi se po zapnuti
829 cizelu 109 {
741 cizelu 110 output_low(SDIN);
111 output_low(SCLK);
842 cizelu 112 olsa_pulses(30); // reset radkoveho senzoru
741 cizelu 113 output_high(SDIN);
842 cizelu 114 olsa_pulses(10); // start bit - synchronizace
741 cizelu 115 olsa_send(main_reset);
116 olsa_pulses(5);
117 olsa_send(set_mode_rg);
118 olsa_send(clear_mode_rg);
829 cizelu 119 }
789 cizelu 120  
842 cizelu 121 void olsa_setup() // kompletni nastaveni, provadi se po resetu
829 cizelu 122 {
842 cizelu 123 olsa_send(left_offset); // nastaveni leveho segmentu (offset a zisk)
789 cizelu 124 olsa_send(offset);
125 olsa_send(left_gain);
126 olsa_send(gain);
842 cizelu 127 olsa_send(mid_offset); // nastaveni prostredniho segmentu (offset a zisk)
789 cizelu 128 olsa_send(offset);
129 olsa_send(mid_gain);
130 olsa_send(gain);
842 cizelu 131 olsa_send(right_offset); // nastaveni praveho segmentu (offset a zisk)
789 cizelu 132 olsa_send(offset);
133 olsa_send(right_gain);
134 olsa_send(gain);
829 cizelu 135 }
790 cizelu 136  
842 cizelu 137 void olsa_integration() // snimani pixelu
829 cizelu 138 {
842 cizelu 139 olsa_send(start_int); // zacatek integrace senzoru
790 cizelu 140 olsa_pulses(22);
842 cizelu 141 olsa_send(stop_int); // konec integrace senzoru
790 cizelu 142 olsa_pulses(5);
829 cizelu 143 }
796 cizelu 144  
837 cizelu 145 void read_olsa()
146 {
842 cizelu 147 int8 cpixel; // pocet prectenych pixelu
148 int8 cbit; // pocet prectenych bitu
149 int8 pixel; // hodnota precteneho pixelu
837 cizelu 150 cpixel=0;
151 lp=0;
152 rp=0;
153 olsa_integration();
154 olsa_send(readout);
842 cizelu 155 do // precte 102 pixelu
837 cizelu 156 {
842 cizelu 157 if(!SDOUT) // zacatek prenosu - zachycen start bit
837 cizelu 158 {
159 pixel=0;
842 cizelu 160 for(cbit=0;cbit<8;cbit++) // cte jednotlive bity (8 bitu - 0 az 7)
837 cizelu 161 {
842 cizelu 162 olsa_pulse(); // impulz pro generovani dalsiho bitu
163 if(SDOUT) // zachycena 1
837 cizelu 164 {
842 cizelu 165 pixel|=1; // zapise do bitu pixelu 1 - OR
837 cizelu 166 }
842 cizelu 167 else // zachycena 0
837 cizelu 168 {
842 cizelu 169 pixel|=0; // zapise do bitu pixelu 0 - OR
837 cizelu 170 }
842 cizelu 171 pixel<<=1; // posune pixel
837 cizelu 172 }
842 cizelu 173 olsa_pulse(); // generuje stop bit
174 if(cpixel<52) // ulozeni do pole
837 cizelu 175 {
842 cizelu 176 olsa_lseg[lp]=pixel; // leva polovina radky - leve pole
837 cizelu 177 lp++;
178 }
179 else
180 {
842 cizelu 181 olsa_rseg[rp]=pixel; // prava polovina cary - prave pole
837 cizelu 182 rp++;
183 }
184 cpixel++;
185 }
186 else
187 {
842 cizelu 188 olsa_pulse(); // generuje start bit, nebyl-li poslan
837 cizelu 189 }
190 }
842 cizelu 191 while(cpixel<102); // precte 102 pixelu
837 cizelu 192 }
193  
741 cizelu 194 //ZACHRANNE SENZORY
842 cizelu 195 void read_blue_sensors() // cteni nouzovych senzoru
829 cizelu 196 {
842 cizelu 197 set_adc_channel(LINEL); // cti levy nouzovy senzor
700 cizelu 198 delay_us(10);
745 cizelu 199 line_l=read_adc();
842 cizelu 200 set_adc_channel(LINER); // cti pravy nouzovy senzor
700 cizelu 201 delay_us(10);
202 line_r=read_adc();
829 cizelu 203 }
730 cizelu 204  
700 cizelu 205 //PIPAK
716 cizelu 206 void beep(int16 period,int16 length)
829 cizelu 207 {
842 cizelu 208 int16 bp; //promenna pro nastaveni delky
700 cizelu 209 for(bp=length;bp>0;bp--)
829 cizelu 210 {
730 cizelu 211 output_high(SOUND_HI);
212 output_low(SOUND_LO);
213 delay_us(period);
214 output_high(SOUND_LO);
215 output_low(SOUND_HI);
216 delay_us(period);
829 cizelu 217 }
218 }
708 cizelu 219 //MOTORY
842 cizelu 220 void l_motor_fwd(int8 speedl) // levy motor dopredu
829 cizelu 221 {
708 cizelu 222 output_high(LMF);
223 output_low(LMB);
224 set_pwm2_duty(speedl);
829 cizelu 225 }
700 cizelu 226  
842 cizelu 227 void l_motor_bwd(int8 speedl) // levy motor dozadu
829 cizelu 228 {
708 cizelu 229 output_high(LMB);
230 output_low(LMF);
231 set_pwm2_duty(speedl);
829 cizelu 232 }
708 cizelu 233  
842 cizelu 234 void r_motor_fwd(int8 speedr) // pravy motor dopredu
829 cizelu 235 {
708 cizelu 236 output_high(RMF);
237 output_low(RMB);
238 set_pwm1_duty(speedr);
829 cizelu 239 }
708 cizelu 240  
842 cizelu 241 void r_motor_bwd(int8 speedr) // pravy motor dozadu
829 cizelu 242 {
708 cizelu 243 output_high(RMB);
244 output_low(RMF);
245 set_pwm1_duty(speedr);
829 cizelu 246 }
708 cizelu 247  
842 cizelu 248 void l_motor_off() // levy motor vypnut
829 cizelu 249 {
708 cizelu 250 output_low(LMF);
251 output_low(LMB);
252 set_pwm2_duty(0);
829 cizelu 253 }
730 cizelu 254  
842 cizelu 255 void r_motor_off() // pravy motor vypnut
829 cizelu 256 {
708 cizelu 257 output_low(RMF);
258 output_low(RMB);
259 set_pwm1_duty(0);
829 cizelu 260 }
708 cizelu 261  
842 cizelu 262 void motor_test() // test motoru
829 cizelu 263 {
708 cizelu 264 int8 i;
716 cizelu 265 beep(100,200);
799 cizelu 266 printf("TEST MOTORU\r\n");
708 cizelu 267 delay_ms(1000);
799 cizelu 268 printf("LEVY MOTOR DOPREDU\r\n");
818 cizelu 269 delay_ms(1000);
708 cizelu 270 for(i=0;i<255;i++)
829 cizelu 271 {
708 cizelu 272 l_motor_fwd(i);
799 cizelu 273 printf("RYCHLOST: %u\r\n",i);
716 cizelu 274 delay_ms(5);
829 cizelu 275 }
708 cizelu 276 for(i=255;i>0;i--)
829 cizelu 277 {
708 cizelu 278 l_motor_fwd(i);
799 cizelu 279 printf("RYCHLOST: %u\r\n",i);
716 cizelu 280 delay_ms(5);
829 cizelu 281 }
799 cizelu 282 printf("LEVY MOTOR DOZADU\r\n");
818 cizelu 283 delay_ms(1000);
708 cizelu 284 for(i=0;i<255;i++)
829 cizelu 285 {
708 cizelu 286 l_motor_bwd(i);
799 cizelu 287 printf("RYCHLOST: %u\r\n",i);
716 cizelu 288 delay_ms(5);
829 cizelu 289 }
708 cizelu 290 for(i=255;i>0;i--)
829 cizelu 291 {
708 cizelu 292 l_motor_bwd(i);
799 cizelu 293 printf("RYCHLOST: %u\r\n",i);
716 cizelu 294 delay_ms(5);
829 cizelu 295 }
799 cizelu 296 printf("PRAVY MOTOR DOPREDU\r\n");
818 cizelu 297 delay_ms(1000);
708 cizelu 298 for(i=0;i<255;i++)
829 cizelu 299 {
708 cizelu 300 r_motor_fwd(i);
799 cizelu 301 printf("RYCHLOST: %u\r\n",i);
716 cizelu 302 delay_ms(5);
829 cizelu 303 }
708 cizelu 304 for(i=255;i>0;i--)
829 cizelu 305 {
708 cizelu 306 r_motor_fwd(i);
799 cizelu 307 printf("RYCHLOST: %u\r\n",i);
716 cizelu 308 delay_ms(5);
829 cizelu 309 }
799 cizelu 310 printf("PRAVY MOTOR DOZADU\r\n");
818 cizelu 311 delay_ms(1000);
708 cizelu 312 for(i=0;i<255;i++)
829 cizelu 313 {
708 cizelu 314 r_motor_bwd(i);
799 cizelu 315 printf("RYCHLOST: %u\r\n",i);
716 cizelu 316 delay_ms(5);
829 cizelu 317 }
708 cizelu 318 for(i=255;i>0;i--)
829 cizelu 319 {
708 cizelu 320 r_motor_bwd(i);
799 cizelu 321 printf("RYCHLOST: %u\r\n",i);
716 cizelu 322 delay_ms(5);
829 cizelu 323 }
818 cizelu 324 l_motor_off();
325 r_motor_off();
799 cizelu 326 printf("KONEC TESTU MOTORU\r\n");
818 cizelu 327 delay_ms(1000);
829 cizelu 328 }
708 cizelu 329  
843 cizelu 330 void diag() // diagnostika - vypis senzoru s moznosti prepnuti na test motoru
708 cizelu 331 {
332 read_blue_sensors();
842 cizelu 333 printf("LEVA: %u \t",line_l);
334 delay_ms(10);
335 printf("PRAVA: %u \t",line_r);
336 delay_ms(10);
337 printf("L_NARAZ: %u \t",BUMPL);
338 delay_ms(10);
339 printf("P_NARAZ: %u \r\n",BUMPR);
340 delay_ms(10);
341 if(BUMPL&&BUMPR) // po zmacknuti stran narazniku spusti test motoru
708 cizelu 342 {
843 cizelu 343 beep(100,200);
344 printf("Levy naraznik - test OLSA\r\n");
345 printf("Pravy naraznik - test motoru\r\n");
346 while(true)
347 {
348 if(BUMPR)
349 {
350 motor_test();
351 }
352 if(BUMPL)
353 {
354 beep(100,200);
355 printf("TEST OLSA\r\n");
356 while(true)
357 {
358 int8 tisk;
359 int8 *tiskp;
360 read_olsa();
361 printf("cteni\r\n"); // po precteni vsech pixelu odradkuje
362 for(tiskp=0;tiskp<52;tiskp++) // tisk leve casti radky
363 {
364 tisk=olsa_lseg[tiskp];
365 printf("%x ",tisk);
366 }
367 for(tiskp=0;tiskp<52;tiskp++) // tisk prave casti radky
368 {
369 tisk=olsa_rseg[tiskp];
370 printf("%x ",tisk);
371 }
372 }
373 }
374 }
708 cizelu 375 }
376 }
377  
700 cizelu 378 // HLAVNI SMYCKA
842 cizelu 379 void main()
829 cizelu 380 {
799 cizelu 381 printf("POWER ON \r\n");
842 cizelu 382 // NASTAVENI > provede se pouze pri zapnuti
383 setup_adc_ports(sAN0-sAN1-sAN2);
789 cizelu 384 setup_adc(ADC_CLOCK_INTERNAL); // interni hodniny pro AD prevodnik
842 cizelu 385 setup_spi(SPI_SS_DISABLED);
386 setup_timer_0(RTCC_INTERNAL|RTCC_DIV_1);
387 setup_timer_1(T1_DISABLED);
388 setup_timer_2(T2_DIV_BY_16,255,1); // casovac pro PWM
789 cizelu 389 setup_ccp1(CCP_PWM); // povoli PWM na pinu RC2
390 setup_ccp2(CCP_PWM); // povolí PWM na pinu RC1
842 cizelu 391 setup_comparator(NC_NC_NC_NC);
392 setup_vref(FALSE);
789 cizelu 393 l_motor_off(); // vypne levy motor
394 r_motor_off(); // vypne pravy motor
395 output_high(LED1); // zhasne LED1
396 output_high(LED2); // zhasne LED2
842 cizelu 397 olsa_reset();
398 olsa_setup();
399 beep(500,200); // pipni pri startu
400 printf("OK! \r\n");
401 delay_ms(500);
402 printf("VYBRAT MOD... \r\n");
403 while(true)
404 {
837 cizelu 405 read_olsa();
730 cizelu 406 }
829 cizelu 407 }