Rev 840 Rev 842
Line 1... Line 1...
1 #include "main.h" 1 #include "main.h"
-   2  
2 // NEPOUZIVAT PINY B6 A B7, JSOU VYHRAZENY PRO SERIOVOU KOMUNIKACI 3 // NEPOUZIVAT PINY B6 A B7, JSOU VYHRAZENY PRO SERIOVOU KOMUNIKACI
3 // BAUD RATE = 9600 Bd/s 4 // BAUD RATE = 9600
-   5  
4 // LED DIODY 6 // univerzalni LED diody
5 #define LED1 PIN_E0 7 #define LED1 PIN_E0
6 #define LED2 PIN_E1 8 #define LED2 PIN_E1
7   9  
8 // PIEZO 10 // piezo pipak
9 #DEFINE SOUND_HI PIN_B4 11 #DEFINE SOUND_HI PIN_B4
10 #DEFINE SOUND_LO PIN_B5 12 #DEFINE SOUND_LO PIN_B5
11   13  
12 // RADKOVY SENZOR - OLSA 14 // radkovy senzor
13 #define SDIN PIN_D4 // seriovy vstup 15 #define SDIN PIN_D4 // seriovy vstup
14 #define SDOUT input(PIN_C5) // seriovy vystup 16 #define SDOUT input(PIN_C5) // seriovy vystup
15 #define SCLK PIN_D5 // takt 17 #define SCLK PIN_D5 // takt
16   18  
17 #define LEVEL_O 96 // rozhodovaci uroven pro olsa - odpovida cerne care 19 #define OLSA_LEV 10 // rozhodovaci uroven (cca 10 odpovida cerne)
18   20  
19 // KOMUNIKACE S OLSA - prvni poslat LSB 21 // pro komunikaci s OLSA, prvni se posila LSB
20 int main_reset[8]={1,1,0,1,1,0,0,0}; // hlavni reset 0x1B 22 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 23 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 24 int clear_mode_rg[8]={0,0,0,0,0,0,0,0}; // nulovani MODE registru 0x00
23   25  
24 int left_offset[8]={0,0,0,0,0,0,1,0}; // offset leveho segmentu senzoru 0x40 26 int left_offset[8]={0,0,0,0,0,0,1,0}; // offset leveho segmentu senzoru 0x40
Line 38... Line 40...
38 int olsa_lseg[51]={0}; // leva cast radky (pixely 0 - 50) 40 int olsa_lseg[51]={0}; // leva cast radky (pixely 0 - 50)
39 int olsa_rseg[51]={0}; // prava cast radky (pixely 51 - 101) 41 int olsa_rseg[51]={0}; // prava cast radky (pixely 51 - 101)
40 int8 *lp; // ukazatel pro levou polovinu radky 42 int8 *lp; // ukazatel pro levou polovinu radky
41 int8 *rp; // ukazatel pro levou polovinu radky 43 int8 *rp; // ukazatel pro levou polovinu radky
42   44  
43 //NARAZNIK 45 //naraznik
44 #define BUMPL input(PIN_D6) 46 #define BUMPL input(PIN_D6)
45 #define BUMPR input(PIN_D7) 47 #define BUMPR input(PIN_D7)
46   48  
47 //MODRE SENZORY - nouzove 49 //nouzove senzory
48 #define LINEL 0 50 #define LINEL 0
49 #define LINER 1 51 #define LINER 1
50 #define TRESHOLD 200 // rozhodovaci uroven (zmereno 0 - cerna, 255 cerna) 52 #define BLUE_LEV 200 // rozhodovaci uroven (zmereno 0 - cerna, 255 cerna)
51 int8 line_l; 53 int8 line_l;
52 int8 line_r; 54 int8 line_r;
53 int1 line_position; 55 int1 line_position;
54   56  
55 // MOTORY - pro rychlost použity piny C1 (pravy motor) a C2 (levy motor) 57 // motory
56 #define LMF PIN_D0 58 #define LMF PIN_D0
57 #define LMB PIN_D1 59 #define LMB PIN_D1
58 #define RMF PIN_D2 60 #define RMF PIN_D2
59 #define RMB PIN_D3 61 #define RMB PIN_D3
60 // NASTAVENI RYCHLOSTI - pro vytvorene funkce -  
-   62  
61 int8 lm_speed; 63 int8 lm_speed;
62 int8 rm_speed; 64 int8 rm_speed;
63   65  
64 //PODPROGRAMY 66 //PODPROGRAMY
65 //SENZORY 67 //SENZORY
66 //OLSA01A 68 //OLSA01A
67 void olsa_pulses(int count) // vytvori impulzy pro ridici logiku 69 void olsa_pulses(int count) // vytvori impulzy pro ridici logiku
68 { 70 {
69 int8 ct; 71 int8 ct;
70 for(ct=0;ct<=count;ct++) 72 for(ct=0;ct<=count;ct++)
71 { 73 {
72 output_high(SCLK); 74 output_high(SCLK);
73 output_low(SCLK); 75 output_low(SCLK);
74 } 76 }
75 } 77 }
76   78  
77 void olsa_pulse() // vytvori jeden impulz 79 void olsa_pulse() // vytvori jeden impulz
78 { 80 {
79 output_high(SCLK); 81 output_high(SCLK);
80 output_low(SCLK); 82 output_low(SCLK);
81 } 83 }
82   84  
83 void olsa_send(int8 info[8]) // USART komunikace s modulem OLSA01A - poslani zpravy 85 void olsa_send(int8 info[8]) // USART komunikace s modulem OLSA01A - poslani zpravy
84 { 86 {
85 int *ip; // ukazatel na pole s informaci 87 int *ip; // ukazatel na pole s informaci
86 int8 i; // pomocna promenna pro nastaveni 0 nebo 1 na SDIN 88 int8 i; // pomocna promenna pro nastaveni 0 nebo 1 na SDIN
87 output_low(SDIN); // start bit 89 output_low(SDIN); // start bit
88 olsa_pulse(); 90 olsa_pulse();
89 for(ip=0;ip<8;ip++) // predani informace - 8 bit, LSB prvni > MSB posledni 91 for(ip=0;ip<8;ip++) // predani informace - 8 bit, LSB prvni > MSB posledni
90 { 92 {
91 i=info[ip]; // ziskani hodnoty z pole 93 i=info[ip]; // ziskani hodnoty z pole
92 if(i==1) // vyhodnoceni obsahu informace - nastav 1 94 if(i==1) // vyhodnoceni obsahu informace - nastav 1
93 { 95 {
94 output_high(SDIN); 96 output_high(SDIN);
95 } 97 }
96 else // vyhodnoceni obsahu informace - nastav 0 98 else // vyhodnoceni obsahu informace - nastav 0
97 { 99 {
98 output_low(SDIN); 100 output_low(SDIN);
99 } 101 }
100 olsa_pulse(); 102 olsa_pulse();
101 } 103 }
102 output_high(SDIN); // stop bit 104 output_high(SDIN); // stop bit
103 olsa_pulse(); 105 olsa_pulse();
104 } 106 }
105   107  
106 void olsa_reset() // hlavni RESET - provadi se po zapnuti 108 void olsa_reset() // hlavni RESET - provadi se po zapnuti
107 { 109 {
108 output_low(SDIN); 110 output_low(SDIN);
109 output_low(SCLK); 111 output_low(SCLK);
110 olsa_pulses(30); // reset radkoveho senzoru 112 olsa_pulses(30); // reset radkoveho senzoru
111 output_high(SDIN); 113 output_high(SDIN);
112 olsa_pulses(10); // start bit - synchronizace 114 olsa_pulses(10); // start bit - synchronizace
113 olsa_send(main_reset); 115 olsa_send(main_reset);
114 olsa_pulses(5); 116 olsa_pulses(5);
115 olsa_send(set_mode_rg); 117 olsa_send(set_mode_rg);
116 olsa_send(clear_mode_rg); 118 olsa_send(clear_mode_rg);
117 } 119 }
118 120
119 void olsa_setup() // kompletni nastaveni, provadi se po resetu 121 void olsa_setup() // kompletni nastaveni, provadi se po resetu
120 { 122 {
121 olsa_send(left_offset); // nastaveni leveho segmentu (offset a zisk) 123 olsa_send(left_offset); // nastaveni leveho segmentu (offset a zisk)
122 olsa_send(offset); 124 olsa_send(offset);
123 olsa_send(left_gain); 125 olsa_send(left_gain);
124 olsa_send(gain); 126 olsa_send(gain);
125 olsa_send(mid_offset); // nastaveni prostredniho segmentu (offset a zisk) 127 olsa_send(mid_offset); // nastaveni prostredniho segmentu (offset a zisk)
126 olsa_send(offset); 128 olsa_send(offset);
127 olsa_send(mid_gain); 129 olsa_send(mid_gain);
128 olsa_send(gain); 130 olsa_send(gain);
129 olsa_send(right_offset); // nastaveni praveho segmentu (offset a zisk) 131 olsa_send(right_offset); // nastaveni praveho segmentu (offset a zisk)
130 olsa_send(offset); 132 olsa_send(offset);
131 olsa_send(right_gain); 133 olsa_send(right_gain);
132 olsa_send(gain); 134 olsa_send(gain);
133 } 135 }
134 136
135 void olsa_integration() // snimani pixelu 137 void olsa_integration() // snimani pixelu
136 { 138 {
137 olsa_send(start_int); // zacatek integrace senzoru 139 olsa_send(start_int); // zacatek integrace senzoru
138 olsa_pulses(22); 140 olsa_pulses(22);
139 olsa_send(stop_int); // konec integrace senzoru 141 olsa_send(stop_int); // konec integrace senzoru
140 olsa_pulses(5); 142 olsa_pulses(5);
141 } 143 }
142   144  
143 void read_olsa() 145 void read_olsa()
144 { 146 {
145 int8 cpixel; // pocet prectenych pixelu 147 int8 cpixel; // pocet prectenych pixelu
146 int8 cbit; // pocet prectenych bitu 148 int8 cbit; // pocet prectenych bitu
147 int8 pixel; // hodnota precteneho pixelu 149 int8 pixel; // hodnota precteneho pixelu
148 cpixel=0; 150 cpixel=0;
149 lp=0; 151 lp=0;
150 rp=0; 152 rp=0;
151 olsa_integration(); 153 olsa_integration();
152 olsa_send(readout); 154 olsa_send(readout);
153 do // precte 102 pixelu 155 do // precte 102 pixelu
154 { 156 {
155 if(!SDOUT) // zacatek prenosu - zachycen start bit 157 if(!SDOUT) // zacatek prenosu - zachycen start bit
156 { 158 {
157 pixel=0; 159 pixel=0;
158 for(cbit=0;cbit<8;cbit++) // cte jednotlive bity (8 bitu - 0 az 7) 160 for(cbit=0;cbit<8;cbit++) // cte jednotlive bity (8 bitu - 0 az 7)
159 { 161 {
160 olsa_pulse(); // impulz pro generovani dalsiho bitu 162 olsa_pulse(); // impulz pro generovani dalsiho bitu
161 if(SDOUT) // zachycena 1 163 if(SDOUT) // zachycena 1
162 { 164 {
163 pixel|=1; // zapise do bitu pixelu 1 - OR 165 pixel|=1; // zapise do bitu pixelu 1 - OR
164 } 166 }
165 else // zachycena 0 167 else // zachycena 0
166 { 168 {
167 pixel|=0; // zapise do bitu pixelu 0 - OR 169 pixel|=0; // zapise do bitu pixelu 0 - OR
168 } 170 }
169 pixel<<=1; // posune pixel 171 pixel<<=1; // posune pixel
170 } 172 }
171 olsa_pulse(); // generuje stop bit 173 olsa_pulse(); // generuje stop bit
172 if(cpixel<52) // ulozeni do pole 174 if(cpixel<52) // ulozeni do pole
173 { 175 {
174 olsa_lseg[lp]=pixel; // leva polovina radky - leve pole 176 olsa_lseg[lp]=pixel; // leva polovina radky - leve pole
175 lp++; 177 lp++;
176 } 178 }
177 else 179 else
178 { 180 {
179 olsa_rseg[rp]=pixel; // prava polovina cary - prave pole 181 olsa_rseg[rp]=pixel; // prava polovina cary - prave pole
180 rp++; 182 rp++;
181 } 183 }
182 cpixel++; 184 cpixel++;
183 } 185 }
184 else 186 else
185 { 187 {
186 olsa_pulse(); // generuje start bit, nebyl-li poslan 188 olsa_pulse(); // generuje start bit, nebyl-li poslan
187 } 189 }
188 } 190 }
189 while(cpixel<102); // precte 102 pixelu 191 while(cpixel<102); // precte 102 pixelu
190 } 192 }
191   193  
192 //ZACHRANNE SENZORY 194 //ZACHRANNE SENZORY
193 void read_blue_sensors() // cteni nouzovych senzoru 195 void read_blue_sensors() // cteni nouzovych senzoru
194 { 196 {
195 set_adc_channel(LINEL); // cti levy nouzovy senzor 197 set_adc_channel(LINEL); // cti levy nouzovy senzor
196 delay_us(10); 198 delay_us(10);
197 line_l=read_adc(); 199 line_l=read_adc();
198 set_adc_channel(LINER); // cti pravy nouzovy senzor 200 set_adc_channel(LINER); // cti pravy nouzovy senzor
199 delay_us(10); 201 delay_us(10);
200 line_r=read_adc(); 202 line_r=read_adc();
201 } 203 }
202 204
203 //PIPAK 205 //PIPAK
204 void beep(int16 period,int16 length) 206 void beep(int16 period,int16 length)
205 { 207 {
206 int16 bp; //promenna pro nastaveni delky 208 int16 bp; //promenna pro nastaveni delky
207 for(bp=length;bp>0;bp--) 209 for(bp=length;bp>0;bp--)
208 { 210 {
209 output_high(SOUND_HI); 211 output_high(SOUND_HI);
210 output_low(SOUND_LO); 212 output_low(SOUND_LO);
211 delay_us(period); 213 delay_us(period);
Line 213... Line 215...
213 output_low(SOUND_HI); 215 output_low(SOUND_HI);
214 delay_us(period); 216 delay_us(period);
215 } 217 }
216 } 218 }
217 //MOTORY 219 //MOTORY
218 void l_motor_fwd(int8 speedl) // levy motor dopredu 220 void l_motor_fwd(int8 speedl) // levy motor dopredu
219 { 221 {
220 output_high(LMF); 222 output_high(LMF);
221 output_low(LMB); 223 output_low(LMB);
222 set_pwm2_duty(speedl); 224 set_pwm2_duty(speedl);
223 } 225 }
224   226  
225 void l_motor_bwd(int8 speedl) // levy motor dozadu 227 void l_motor_bwd(int8 speedl) // levy motor dozadu
226 { 228 {
227 output_high(LMB); 229 output_high(LMB);
228 output_low(LMF); 230 output_low(LMF);
229 set_pwm2_duty(speedl); 231 set_pwm2_duty(speedl);
230 } 232 }
231   233  
232 void r_motor_fwd(int8 speedr) // pravy motor dopredu 234 void r_motor_fwd(int8 speedr) // pravy motor dopredu
233 { 235 {
234 output_high(RMF); 236 output_high(RMF);
235 output_low(RMB); 237 output_low(RMB);
236 set_pwm1_duty(speedr); 238 set_pwm1_duty(speedr);
237 } 239 }
238   240  
239 void r_motor_bwd(int8 speedr) // pravy motor dozadu 241 void r_motor_bwd(int8 speedr) // pravy motor dozadu
240 { 242 {
241 output_high(RMB); 243 output_high(RMB);
242 output_low(RMF); 244 output_low(RMF);
243 set_pwm1_duty(speedr); 245 set_pwm1_duty(speedr);
244 } 246 }
245   247  
246 void l_motor_off() // levy motor vypnut 248 void l_motor_off() // levy motor vypnut
247 { 249 {
248 output_low(LMF); 250 output_low(LMF);
249 output_low(LMB); 251 output_low(LMB);
250 set_pwm2_duty(0); 252 set_pwm2_duty(0);
251 } 253 }
252 254
253 void r_motor_off() // pravy motor vypnut 255 void r_motor_off() // pravy motor vypnut
254 { 256 {
255 output_low(RMF); 257 output_low(RMF);
256 output_low(RMB); 258 output_low(RMB);
257 set_pwm1_duty(0); 259 set_pwm1_duty(0);
258 } 260 }
259   261  
260 void motor_test() // test motoru 262 void motor_test() // test motoru
261 { 263 {
262 int8 i; 264 int8 i;
263 beep(100,200); 265 beep(100,200);
264 printf("TEST MOTORU\r\n"); 266 printf("TEST MOTORU\r\n");
265 delay_ms(1000); 267 delay_ms(1000);
Line 323... Line 325...
323 r_motor_off(); 325 r_motor_off();
324 printf("KONEC TESTU MOTORU\r\n"); 326 printf("KONEC TESTU MOTORU\r\n");
325 delay_ms(1000); 327 delay_ms(1000);
326 } 328 }
327   329  
328 void diag() // diagnostika - vypis senzoru s moznosti prepnuti na test motoru 330 void diagnostika() // diagnostika - vypis senzoru s moznosti prepnuti na test motoru
329 { 331 {
330 read_blue_sensors(); 332 read_blue_sensors();
331 printf("LEVA: %u \t",line_l); 333 printf("LEVA: %u \t",line_l);
-   334 delay_ms(10);
332 printf("PRAVA: %u \t",line_r); 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);
333 if(BUMPL) // leva strana narazniku pusti test motoru 341 if(BUMPL&&BUMPR) // po zmacknuti stran narazniku spusti test motoru
334 { 342 {
335 motor_test(); 343 motor_test();
336 } 344 }
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 } -  
354 } 345 }
355   346  
356 // HLAVNI SMYCKA 347 // HLAVNI SMYCKA
357 void main() // NASTAVENI > provede se pouze pri zapnuti 348 void main()
358 { 349 {
359 printf("POWER ON \r\n"); 350 printf("POWER ON \r\n");
-   351 // NASTAVENI > provede se pouze pri zapnuti
360 setup_adc_ports(sAN0-sAN1-sAN2); // pouzity analogove vstupy A0, A1 a A3 352 setup_adc_ports(sAN0-sAN1-sAN2);
361 setup_adc(ADC_CLOCK_INTERNAL); // interni hodniny pro AD prevodnik 353 setup_adc(ADC_CLOCK_INTERNAL); // interni hodniny pro AD prevodnik
362 setup_spi(SPI_SS_DISABLED); // komunikace pres SPI zakazana 354 setup_spi(SPI_SS_DISABLED);
363 setup_timer_0(RTCC_INTERNAL|RTCC_DIV_1); 355 setup_timer_0(RTCC_INTERNAL|RTCC_DIV_1);
364 setup_timer_1(T1_DISABLED); // casovac T1 nepouzit 356 setup_timer_1(T1_DISABLED);
365 setup_timer_2(T2_DIV_BY_16,255,1); // casovac T2 pro PWM 357 setup_timer_2(T2_DIV_BY_16,255,1); // casovac pro PWM
366 setup_ccp1(CCP_PWM); // povoli PWM na pinu RC2 358 setup_ccp1(CCP_PWM); // povoli PWM na pinu RC2
367 setup_ccp2(CCP_PWM); // povolí PWM na pinu RC1 359 setup_ccp2(CCP_PWM); // povolí PWM na pinu RC1
368 setup_comparator(NC_NC_NC_NC); // vypnuti vsech komparatoru 360 setup_comparator(NC_NC_NC_NC);
369 setup_vref(FALSE); // nepouziva se referencni napeti 361 setup_vref(FALSE);
370 l_motor_off(); // vypne levy motor 362 l_motor_off(); // vypne levy motor
371 r_motor_off(); // vypne pravy motor 363 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 364 output_high(LED1); // zhasne LED1
375 output_high(LED2); // zhasne LED2 365 output_high(LED2); // zhasne LED2
376 olsa_reset(); // provede reset senzoru - nuno vzdy po zapnuti 366 olsa_reset();
-   367 olsa_setup();
377 olsa_setup(); // provede pocatecni nastaveni OLSA 368 beep(500,200); // pipni pri startu
378 printf("Nastaveni provedeno! \r\n"); 369 printf("OK! \r\n");
379 beep(500,200); // pipni po ukonceni nastaveni 370 delay_ms(500);
380 printf("Zvolte mod. \r\n"); 371 printf("VYBRAT MOD... \r\n");
381 while(true) // RIDICI SMYCKA 372 while(true)
382 { 373 {
-   374 int8 tisk;
-   375 int8 *tiskp;
383 read_olsa(); 376 read_olsa();
-   377 printf("cteni\r\n"); // po precteni vsech pixelu posle "enter"
-   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
384 } 389 }
385 } 390 }