Rev Author Line No. Line
799 cizelu 1 #include "main.h"
708 cizelu 2 // NEPOUZIVAT PINY B6 A B7, JSOU VYHRAZENY PRO SERIOVOU KOMUNIKACI
840 cizelu 3 // BAUD RATE = 9600 Bd/s
4 // LED DIODY
830 cizelu 5 #define LED1 PIN_E0
6 #define LED2 PIN_E1
700 cizelu 7  
840 cizelu 8 // PIEZO
716 cizelu 9 #DEFINE SOUND_HI PIN_B4
10 #DEFINE SOUND_LO PIN_B5
700 cizelu 11  
840 cizelu 12 // RADKOVY SENZOR - OLSA
745 cizelu 13 #define SDIN PIN_D4 // seriovy vstup
830 cizelu 14 #define SDOUT input(PIN_C5) // seriovy vystup
745 cizelu 15 #define SCLK PIN_D5 // takt
727 cizelu 16  
840 cizelu 17 #define LEVEL_O 96 // rozhodovaci uroven pro olsa - odpovida cerne care
18  
19 // KOMUNIKACE S OLSA - prvni poslat 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  
840 cizelu 43 //NARAZNIK
708 cizelu 44 #define BUMPL input(PIN_D6)
45 #define BUMPR input(PIN_D7)
700 cizelu 46  
840 cizelu 47 //MODRE SENZORY - nouzove
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  
840 cizelu 55 // MOTORY - pro rychlost použity piny C1 (pravy motor) a C2 (levy motor)
708 cizelu 56 #define LMF PIN_D0
57 #define LMB PIN_D1
58 #define RMF PIN_D2
59 #define RMB PIN_D3
840 cizelu 60 // NASTAVENI RYCHLOSTI - pro vytvorene funkce
61 int8 lm_speed;
790 cizelu 62 int8 rm_speed;
63  
700 cizelu 64 //PODPROGRAMY
65 //SENZORY
730 cizelu 66 //OLSA01A
840 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  
840 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  
840 cizelu 83 void olsa_send(int8 info[8]) // USART komunikace s modulem OLSA01A - poslani zpravy
829 cizelu 84 {
840 cizelu 85 int *ip; // ukazatel na pole s informaci
86 int8 i; // pomocna promenna pro nastaveni 0 nebo 1 na SDIN
87 output_low(SDIN); // start bit
741 cizelu 88 olsa_pulse();
840 cizelu 89 for(ip=0;ip<8;ip++) // predani informace - 8 bit, LSB prvni > MSB posledni
829 cizelu 90 {
840 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 }
840 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 }
840 cizelu 102 output_high(SDIN); // stop bit
741 cizelu 103 olsa_pulse();
829 cizelu 104 }
730 cizelu 105  
840 cizelu 106 void olsa_reset() // hlavni RESET - provadi se po zapnuti
829 cizelu 107 {
741 cizelu 108 output_low(SDIN);
109 output_low(SCLK);
840 cizelu 110 olsa_pulses(30); // reset radkoveho senzoru
741 cizelu 111 output_high(SDIN);
840 cizelu 112 olsa_pulses(10); // start bit - synchronizace
741 cizelu 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  
840 cizelu 119 void olsa_setup() // kompletni nastaveni, provadi se po resetu
829 cizelu 120 {
840 cizelu 121 olsa_send(left_offset); // nastaveni leveho segmentu (offset a zisk)
789 cizelu 122 olsa_send(offset);
123 olsa_send(left_gain);
124 olsa_send(gain);
840 cizelu 125 olsa_send(mid_offset); // nastaveni prostredniho segmentu (offset a zisk)
789 cizelu 126 olsa_send(offset);
127 olsa_send(mid_gain);
128 olsa_send(gain);
840 cizelu 129 olsa_send(right_offset); // nastaveni praveho segmentu (offset a zisk)
789 cizelu 130 olsa_send(offset);
131 olsa_send(right_gain);
132 olsa_send(gain);
829 cizelu 133 }
790 cizelu 134  
840 cizelu 135 void olsa_integration() // snimani pixelu
829 cizelu 136 {
840 cizelu 137 olsa_send(start_int); // zacatek integrace senzoru
790 cizelu 138 olsa_pulses(22);
840 cizelu 139 olsa_send(stop_int); // konec integrace senzoru
790 cizelu 140 olsa_pulses(5);
829 cizelu 141 }
796 cizelu 142  
837 cizelu 143 void read_olsa()
144 {
840 cizelu 145 int8 cpixel; // pocet prectenych pixelu
146 int8 cbit; // pocet prectenych bitu
147 int8 pixel; // hodnota precteneho pixelu
837 cizelu 148 cpixel=0;
149 lp=0;
150 rp=0;
151 olsa_integration();
152 olsa_send(readout);
840 cizelu 153 do // precte 102 pixelu
837 cizelu 154 {
840 cizelu 155 if(!SDOUT) // zacatek prenosu - zachycen start bit
837 cizelu 156 {
157 pixel=0;
840 cizelu 158 for(cbit=0;cbit<8;cbit++) // cte jednotlive bity (8 bitu - 0 az 7)
837 cizelu 159 {
840 cizelu 160 olsa_pulse(); // impulz pro generovani dalsiho bitu
161 if(SDOUT) // zachycena 1
837 cizelu 162 {
840 cizelu 163 pixel|=1; // zapise do bitu pixelu 1 - OR
837 cizelu 164 }
840 cizelu 165 else // zachycena 0
837 cizelu 166 {
840 cizelu 167 pixel|=0; // zapise do bitu pixelu 0 - OR
837 cizelu 168 }
840 cizelu 169 pixel<<=1; // posune pixel
837 cizelu 170 }
840 cizelu 171 olsa_pulse(); // generuje stop bit
172 if(cpixel<52) // ulozeni do pole
837 cizelu 173 {
840 cizelu 174 olsa_lseg[lp]=pixel; // leva polovina radky - leve pole
837 cizelu 175 lp++;
176 }
177 else
178 {
840 cizelu 179 olsa_rseg[rp]=pixel; // prava polovina cary - prave pole
837 cizelu 180 rp++;
181 }
182 cpixel++;
183 }
184 else
185 {
840 cizelu 186 olsa_pulse(); // generuje start bit, nebyl-li poslan
837 cizelu 187 }
188 }
840 cizelu 189 while(cpixel<102); // precte 102 pixelu
837 cizelu 190 }
191  
741 cizelu 192 //ZACHRANNE SENZORY
840 cizelu 193 void read_blue_sensors() // cteni nouzovych senzoru
829 cizelu 194 {
840 cizelu 195 set_adc_channel(LINEL); // cti levy nouzovy senzor
700 cizelu 196 delay_us(10);
745 cizelu 197 line_l=read_adc();
840 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 {
840 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
840 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  
840 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  
840 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  
840 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  
840 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  
840 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  
840 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  
840 cizelu 328 void diag() // diagnostika - vypis senzoru s moznosti prepnuti na test motoru
708 cizelu 329 {
330 read_blue_sensors();
840 cizelu 331 printf("LEVA: %u \t",line_l);
332 printf("PRAVA: %u \t",line_r);
333 if(BUMPL) // leva strana narazniku pusti test motoru
708 cizelu 334 {
335 motor_test();
336 }
840 cizelu 337 if(BUMPR) // prava strana narazniku precte hodnoty z OLSA
338 {
339 int8 tisk;
340 int8 *tiskp;
341 read_olsa(); // cte hodnoty z olsa a uklada do poli
342 printf("\r\n"); // po precteni vsech pixelu posle "enter"
343 for(tiskp=0;tiskp<52;tiskp++) // tisk leve casti radky
344 {
345 tisk=olsa_lseg[tiskp];
346 printf("%x ",tisk);
347 }
348 for(tiskp=0;tiskp<52;tiskp++) // tisk prave casti radky
349 {
350 tisk=olsa_rseg[tiskp];
351 printf("%x ",tisk);
352 }
353 }
708 cizelu 354 }
355  
700 cizelu 356 // HLAVNI SMYCKA
840 cizelu 357 void main() // NASTAVENI > provede se pouze pri zapnuti
829 cizelu 358 {
799 cizelu 359 printf("POWER ON \r\n");
840 cizelu 360 setup_adc_ports(sAN0-sAN1-sAN2); // pouzity analogove vstupy A0, A1 a A3
789 cizelu 361 setup_adc(ADC_CLOCK_INTERNAL); // interni hodniny pro AD prevodnik
840 cizelu 362 setup_spi(SPI_SS_DISABLED); // komunikace pres SPI zakazana
363 setup_timer_0(RTCC_INTERNAL|RTCC_DIV_1);
364 setup_timer_1(T1_DISABLED); // casovac T1 nepouzit
365 setup_timer_2(T2_DIV_BY_16,255,1); // casovac T2 pro PWM
789 cizelu 366 setup_ccp1(CCP_PWM); // povoli PWM na pinu RC2
367 setup_ccp2(CCP_PWM); // povolí PWM na pinu RC1
840 cizelu 368 setup_comparator(NC_NC_NC_NC); // vypnuti vsech komparatoru
369 setup_vref(FALSE); // nepouziva se referencni napeti
789 cizelu 370 l_motor_off(); // vypne levy motor
371 r_motor_off(); // vypne pravy motor
372 olsa_reset(); // reset logiky radkoveho senzoru
373 olsa_setup(); // nastaveni segmentu radkoveho senzoru (offset a zisk)
374 output_high(LED1); // zhasne LED1
375 output_high(LED2); // zhasne LED2
840 cizelu 376 olsa_reset(); // provede reset senzoru - nuno vzdy po zapnuti
377 olsa_setup(); // provede pocatecni nastaveni OLSA
378 printf("Nastaveni provedeno! \r\n");
379 beep(500,200); // pipni po ukonceni nastaveni
380 printf("Zvolte mod. \r\n");
381 while(true) // RIDICI SMYCKA
382 {
837 cizelu 383 read_olsa();
730 cizelu 384 }
829 cizelu 385 }