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