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