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