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