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