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
830 cizelu 7 #define LED1 PIN_E0
8 #define LED2 PIN_E1
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
830 cizelu 16 #define SDOUT input(PIN_C5) // seriovy vystup
745 cizelu 17 #define SCLK PIN_D5 // takt
727 cizelu 18  
789 cizelu 19 // pro komunikaci s OLSA, prvni se posila 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)
789 cizelu 40 int8 *line_lp=&olsa_lseg; // ukazatel na levou cast radky
41 int8 *line_rp=&olsa_rseg; // ukazatel na pravou cast radky
812 cizelu 42  
700 cizelu 43 //naraznik
708 cizelu 44 #define BUMPL input(PIN_D6)
45 #define BUMPR input(PIN_D7)
700 cizelu 46  
47 //nouzove senzory
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  
55 // motory
708 cizelu 56 #define LMF PIN_D0
57 #define LMB PIN_D1
58 #define RMF PIN_D2
59 #define RMB PIN_D3
700 cizelu 60  
790 cizelu 61 int8 lm_speed;
62 int8 rm_speed;
63  
700 cizelu 64 //PODPROGRAMY
65 //SENZORY
730 cizelu 66 //OLSA01A
745 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  
745 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  
816 cizelu 83 void olsa_send(int8 info[8]) // USART komunikace s modulem OLSA01A - poslani zpravy
829 cizelu 84 {
816 cizelu 85 int *ip; // ukazatel na pole s informaci
86 int8 i; // pomocna promenna pro nastaveni 0 nebo 1 na SDIN
741 cizelu 87 output_low(SDIN); // start bit
88 olsa_pulse();
89 for(ip=0;ip<8;ip++) // predani informace - 8 bit, LSB prvni > MSB posledni
829 cizelu 90 {
741 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 }
741 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 }
741 cizelu 102 output_high(SDIN); // stop bit
103 olsa_pulse();
829 cizelu 104 }
730 cizelu 105  
741 cizelu 106 void olsa_reset() // hlavni RESET - provadi se po zapnuti
829 cizelu 107 {
741 cizelu 108 output_low(SDIN);
109 output_low(SCLK);
110 olsa_pulses(30); // reset radkoveho senzoru
111 output_high(SDIN);
112 olsa_pulses(10); // start bit - synchronizace
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  
119 void olsa_setup() // kompletni nastaveni, provadi se po resetu
829 cizelu 120 {
789 cizelu 121 olsa_send(left_offset); // nastaveni leveho segmentu (offset a zisk)
122 olsa_send(offset);
123 olsa_send(left_gain);
124 olsa_send(gain);
125 olsa_send(mid_offset); // nastaveni prostredniho segmentu (offset a zisk)
126 olsa_send(offset);
127 olsa_send(mid_gain);
128 olsa_send(gain);
129 olsa_send(right_offset); // nastaveni praveho segmentu (offset a zisk)
130 olsa_send(offset);
131 olsa_send(right_gain);
132 olsa_send(gain);
829 cizelu 133 }
790 cizelu 134  
135 void olsa_integration() // snimani pixelu
829 cizelu 136 {
790 cizelu 137 olsa_send(start_int); // zacatek integrace senzoru
138 olsa_pulses(22);
139 olsa_send(stop_int); // konec integrace senzoru
140 olsa_pulses(5);
829 cizelu 141 }
796 cizelu 142  
741 cizelu 143 //ZACHRANNE SENZORY
790 cizelu 144 void read_blue_sensors() // cteni nouzovych senzoru
829 cizelu 145 {
790 cizelu 146 set_adc_channel(LINEL); // cti levy nouzovy senzor
700 cizelu 147 delay_us(10);
745 cizelu 148 line_l=read_adc();
790 cizelu 149 set_adc_channel(LINER); // cti pravy nouzovy senzor
700 cizelu 150 delay_us(10);
151 line_r=read_adc();
829 cizelu 152 }
730 cizelu 153  
700 cizelu 154 //PIPAK
716 cizelu 155 void beep(int16 period,int16 length)
829 cizelu 156 {
790 cizelu 157 int16 bp; //promenna pro nastaveni delky
700 cizelu 158 for(bp=length;bp>0;bp--)
829 cizelu 159 {
730 cizelu 160 output_high(SOUND_HI);
161 output_low(SOUND_LO);
162 delay_us(period);
163 output_high(SOUND_LO);
164 output_low(SOUND_HI);
165 delay_us(period);
829 cizelu 166 }
167 }
708 cizelu 168 //MOTORY
790 cizelu 169 void l_motor_fwd(int8 speedl) // levy motor dopredu
829 cizelu 170 {
708 cizelu 171 output_high(LMF);
172 output_low(LMB);
173 set_pwm2_duty(speedl);
829 cizelu 174 }
700 cizelu 175  
790 cizelu 176 void l_motor_bwd(int8 speedl) // levy motor dozadu
829 cizelu 177 {
708 cizelu 178 output_high(LMB);
179 output_low(LMF);
180 set_pwm2_duty(speedl);
829 cizelu 181 }
708 cizelu 182  
790 cizelu 183 void r_motor_fwd(int8 speedr) // pravy motor dopredu
829 cizelu 184 {
708 cizelu 185 output_high(RMF);
186 output_low(RMB);
187 set_pwm1_duty(speedr);
829 cizelu 188 }
708 cizelu 189  
790 cizelu 190 void r_motor_bwd(int8 speedr) // pravy motor dozadu
829 cizelu 191 {
708 cizelu 192 output_high(RMB);
193 output_low(RMF);
194 set_pwm1_duty(speedr);
829 cizelu 195 }
708 cizelu 196  
790 cizelu 197 void l_motor_off() // levy motor vypnut
829 cizelu 198 {
708 cizelu 199 output_low(LMF);
200 output_low(LMB);
201 set_pwm2_duty(0);
829 cizelu 202 }
730 cizelu 203  
790 cizelu 204 void r_motor_off() // pravy motor vypnut
829 cizelu 205 {
708 cizelu 206 output_low(RMF);
207 output_low(RMB);
208 set_pwm1_duty(0);
829 cizelu 209 }
708 cizelu 210  
790 cizelu 211 void motor_test() // test motoru
829 cizelu 212 {
708 cizelu 213 int8 i;
716 cizelu 214 beep(100,200);
799 cizelu 215 printf("TEST MOTORU\r\n");
708 cizelu 216 delay_ms(1000);
799 cizelu 217 printf("LEVY MOTOR DOPREDU\r\n");
818 cizelu 218 delay_ms(1000);
708 cizelu 219 for(i=0;i<255;i++)
829 cizelu 220 {
708 cizelu 221 l_motor_fwd(i);
799 cizelu 222 printf("RYCHLOST: %u\r\n",i);
716 cizelu 223 delay_ms(5);
829 cizelu 224 }
708 cizelu 225 for(i=255;i>0;i--)
829 cizelu 226 {
708 cizelu 227 l_motor_fwd(i);
799 cizelu 228 printf("RYCHLOST: %u\r\n",i);
716 cizelu 229 delay_ms(5);
829 cizelu 230 }
799 cizelu 231 printf("LEVY MOTOR DOZADU\r\n");
818 cizelu 232 delay_ms(1000);
708 cizelu 233 for(i=0;i<255;i++)
829 cizelu 234 {
708 cizelu 235 l_motor_bwd(i);
799 cizelu 236 printf("RYCHLOST: %u\r\n",i);
716 cizelu 237 delay_ms(5);
829 cizelu 238 }
708 cizelu 239 for(i=255;i>0;i--)
829 cizelu 240 {
708 cizelu 241 l_motor_bwd(i);
799 cizelu 242 printf("RYCHLOST: %u\r\n",i);
716 cizelu 243 delay_ms(5);
829 cizelu 244 }
799 cizelu 245 printf("PRAVY MOTOR DOPREDU\r\n");
818 cizelu 246 delay_ms(1000);
708 cizelu 247 for(i=0;i<255;i++)
829 cizelu 248 {
708 cizelu 249 r_motor_fwd(i);
799 cizelu 250 printf("RYCHLOST: %u\r\n",i);
716 cizelu 251 delay_ms(5);
829 cizelu 252 }
708 cizelu 253 for(i=255;i>0;i--)
829 cizelu 254 {
708 cizelu 255 r_motor_fwd(i);
799 cizelu 256 printf("RYCHLOST: %u\r\n",i);
716 cizelu 257 delay_ms(5);
829 cizelu 258 }
799 cizelu 259 printf("PRAVY MOTOR DOZADU\r\n");
818 cizelu 260 delay_ms(1000);
708 cizelu 261 for(i=0;i<255;i++)
829 cizelu 262 {
708 cizelu 263 r_motor_bwd(i);
799 cizelu 264 printf("RYCHLOST: %u\r\n",i);
716 cizelu 265 delay_ms(5);
829 cizelu 266 }
708 cizelu 267 for(i=255;i>0;i--)
829 cizelu 268 {
708 cizelu 269 r_motor_bwd(i);
799 cizelu 270 printf("RYCHLOST: %u\r\n",i);
716 cizelu 271 delay_ms(5);
829 cizelu 272 }
818 cizelu 273 l_motor_off();
274 r_motor_off();
799 cizelu 275 printf("KONEC TESTU MOTORU\r\n");
818 cizelu 276 delay_ms(1000);
829 cizelu 277 }
708 cizelu 278  
790 cizelu 279 void diagnostika() // diagnostika - vypis senzoru s moznosti prepnuti na test motoru
708 cizelu 280 {
281 read_blue_sensors();
282 printf("LEVA: %u \t",line_l);
789 cizelu 283 delay_ms(10);
708 cizelu 284 printf("PRAVA: %u \t",line_r);
789 cizelu 285 delay_ms(10);
708 cizelu 286 printf("L_NARAZ: %u \t",BUMPL);
789 cizelu 287 delay_ms(10);
799 cizelu 288 printf("P_NARAZ: %u \r\n",BUMPR);
789 cizelu 289 delay_ms(10);
790 cizelu 290 if(BUMPL&&BUMPR) // po zmacknuti stran narazniku spusti test motoru
708 cizelu 291 {
292 motor_test();
293 }
294 }
295  
700 cizelu 296 // HLAVNI SMYCKA
297 void main()
829 cizelu 298 {
799 cizelu 299 printf("POWER ON \r\n");
700 cizelu 300 // NASTAVENI > provede se pouze pri zapnuti
708 cizelu 301 setup_adc_ports(sAN0-sAN1-sAN2);
789 cizelu 302 setup_adc(ADC_CLOCK_INTERNAL); // interni hodniny pro AD prevodnik
700 cizelu 303 setup_spi(SPI_SS_DISABLED);
304 setup_timer_0(RTCC_INTERNAL|RTCC_DIV_1);
305 setup_timer_1(T1_DISABLED);
789 cizelu 306 setup_timer_2(T2_DIV_BY_16,255,1); // casovac pro PWM
307 setup_ccp1(CCP_PWM); // povoli PWM na pinu RC2
308 setup_ccp2(CCP_PWM); // povolí PWM na pinu RC1
700 cizelu 309 setup_comparator(NC_NC_NC_NC);
708 cizelu 310 setup_vref(FALSE);
789 cizelu 311 l_motor_off(); // vypne levy motor
312 r_motor_off(); // vypne pravy motor
313 olsa_reset(); // reset logiky radkoveho senzoru
314 olsa_setup(); // nastaveni segmentu radkoveho senzoru (offset a zisk)
315 output_high(LED1); // zhasne LED1
316 output_high(LED2); // zhasne LED2
829 cizelu 317 olsa_reset();
318 olsa_setup();
789 cizelu 319 beep(500,200); // pipni pri startu
799 cizelu 320 printf("OK! \r\n");
716 cizelu 321 delay_ms(500);
830 cizelu 322 printf("VYBRAT MOD... \r\n");
700 cizelu 323 while(true)
830 cizelu 324 {
833 cizelu 325 int8 cpixel; // pocet prectenych pixelu
326 int8 cbit; // pocet prectenych bitu
327 int8 pixel; // hodnota precteneho pixelu
328 cpixel=0;
834 cizelu 329 olsa_integration();
330 olsa_send(readout);
331 do // precte 102 pixelu
833 cizelu 332 {
333 if(!SDOUT) // zacatek prenosu - zachycen start bit
334 {
335 pixel=0;
336 for(cbit=0;cbit<8;cbit++) // cte jednotlive bity (8 bitu - 0 az 7)
830 cizelu 337 {
833 cizelu 338 olsa_pulse(); // impulz pro generovani dalsiho bitu
339 if(SDOUT) // zachycena 1
340 {
341 pixel|=1; // zapise do bitu pixelu 1 - OR
342 }
343 else // zachycena 0
344 {
345 pixel|=0; // zapise do bitu pixelu 0 - OR
346 }
347 pixel<<=1; // posune pixel
830 cizelu 348 }
833 cizelu 349 olsa_pulse(); // generuje stop bit
350 cpixel++;
834 cizelu 351 printf("%x ",pixel); // tisk na seriovku
833 cizelu 352 }
353 else
354 {
355 olsa_pulse(); // generuje start bit, nebyl-li poslan
356 }
830 cizelu 357 }
834 cizelu 358 while(cpixel<102); // precte 102 pixelu
359 printf("\r\n"); // po precteni vsech pixelu posle "enter"
730 cizelu 360 }
829 cizelu 361 }