Rev Author Line No. Line
708 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
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  
789 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  
789 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  
789 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
37  
38 int olsa_lseg[51]={0}; // leva cast radky (pixely 0 - 50)
39 int olsa_rseg[51]={0}; // prava cast radky (pixely 51 - 102)
40 int8 *line_lp=&olsa_lseg; // ukazatel na levou cast radky
41 int8 *line_rp=&olsa_rseg; // ukazatel na pravou cast radky
42  
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  
62 //PODPROGRAMY
63 //SENZORY
730 cizelu 64 //OLSA01A
745 cizelu 65 void olsa_pulses(int count) // vytvori impulzy pro ridici logiku
730 cizelu 66 {
741 cizelu 67 int8 ct;
68 for(ct=0;ct<=count;ct++)
730 cizelu 69 {
745 cizelu 70 delay_us(1); // doba pro ustaleni
730 cizelu 71 output_high(SCLK);
745 cizelu 72 delay_us(1); // doba pro ustaleni
730 cizelu 73 output_low(SCLK);
74 }
75 }
76  
745 cizelu 77 void olsa_pulse() // vytvori jeden impulz
730 cizelu 78 {
745 cizelu 79 delay_us(1); // doba pro ustaleni
741 cizelu 80 output_high(SCLK);
745 cizelu 81 delay_us(1); // doba pro ustaleni
741 cizelu 82 output_low(SCLK);
83 }
84  
85 void olsa_send(int info[]) // USART komunikace s modulem OLSA01A - poslani zpravy
86 {
789 cizelu 87 int8 *ip=&info; // ukazatel na pole s informaci
88 int i; // pomocna promenna pro nastaveni 0 nebo 1 na SDIN
741 cizelu 89  
90 output_low(SDIN); // start bit
91 olsa_pulse();
92 for(ip=0;ip<8;ip++) // predani informace - 8 bit, LSB prvni > MSB posledni
730 cizelu 93 {
741 cizelu 94 i=info[ip]; // ziskani hodnoty z pole
95 if(i==1) // vyhodnoceni obsahu informace - nastav 1
730 cizelu 96 {
97 output_high(SDIN);
98 }
741 cizelu 99 else // vyhodnoceni obsahu informace - nastav 0
730 cizelu 100 {
101 output_low(SDIN);
102 }
741 cizelu 103 olsa_pulse();
730 cizelu 104 }
741 cizelu 105 output_high(SDIN); // stop bit
106 olsa_pulse();
730 cizelu 107 }
108  
741 cizelu 109 void olsa_reset() // hlavni RESET - provadi se po zapnuti
110 {
111 output_low(SDIN);
112 output_low(SCLK);
113 olsa_pulses(30); // reset radkoveho senzoru
114 output_high(SDIN);
115 olsa_pulses(10); // start bit - synchronizace
116 olsa_send(main_reset);
117 olsa_pulses(5);
118 olsa_send(set_mode_rg);
119 olsa_send(clear_mode_rg);
120 }
789 cizelu 121  
122 void olsa_setup() // kompletni nastaveni, provadi se po resetu
123 {
124 olsa_send(left_offset); // nastaveni leveho segmentu (offset a zisk)
125 olsa_send(offset);
126 olsa_send(left_gain);
127 olsa_send(gain);
128 olsa_send(mid_offset); // nastaveni prostredniho segmentu (offset a zisk)
129 olsa_send(offset);
130 olsa_send(mid_gain);
131 olsa_send(gain);
132 olsa_send(right_offset); // nastaveni praveho segmentu (offset a zisk)
133 olsa_send(offset);
134 olsa_send(right_gain);
135 olsa_send(gain);
136 }
741 cizelu 137  
138 //ZACHRANNE SENZORY
708 cizelu 139 void read_blue_sensors() // cteni nouzovych senzoru
140 {
141 set_adc_channel(LINEL); // cti levy nouzovy senzor
700 cizelu 142 delay_us(10);
745 cizelu 143 line_l=read_adc();
708 cizelu 144 set_adc_channel(LINER); // cti pravy nouzovy senzor
700 cizelu 145 delay_us(10);
146 line_r=read_adc();
708 cizelu 147 }
730 cizelu 148  
700 cizelu 149 //PIPAK
716 cizelu 150 void beep(int16 period,int16 length)
730 cizelu 151 {
716 cizelu 152 int16 bp; //promenna pro nastaveni delky
700 cizelu 153  
154 for(bp=length;bp>0;bp--)
730 cizelu 155 {
156 output_high(SOUND_HI);
157 output_low(SOUND_LO);
158 delay_us(period);
159 output_high(SOUND_LO);
160 output_low(SOUND_HI);
161 delay_us(period);
162 }
163 }
708 cizelu 164 //MOTORY
165 void l_motor_fwd(int8 speedl) // levy motor dopredu
730 cizelu 166 {
708 cizelu 167 output_high(LMF);
168 output_low(LMB);
169 set_pwm2_duty(speedl);
730 cizelu 170 }
700 cizelu 171  
708 cizelu 172 void l_motor_bwd(int8 speedl) // levy motor dozadu
730 cizelu 173 {
708 cizelu 174 output_high(LMB);
175 output_low(LMF);
176 set_pwm2_duty(speedl);
730 cizelu 177 }
708 cizelu 178  
179 void r_motor_fwd(int8 speedr) // pravy motor dopredu
730 cizelu 180 {
708 cizelu 181 output_high(RMF);
182 output_low(RMB);
183 set_pwm1_duty(speedr);
730 cizelu 184 }
708 cizelu 185  
186 void r_motor_bwd(int8 speedr) // pravy motor dozadu
730 cizelu 187 {
708 cizelu 188 output_high(RMB);
189 output_low(RMF);
190 set_pwm1_duty(speedr);
730 cizelu 191 }
708 cizelu 192  
193 void l_motor_off() // levy motor vypnut
730 cizelu 194 {
708 cizelu 195 output_low(LMF);
196 output_low(LMB);
197 set_pwm2_duty(0);
730 cizelu 198 }
199  
708 cizelu 200 void r_motor_off() // pravy motor vypnut
730 cizelu 201 {
708 cizelu 202 output_low(RMF);
203 output_low(RMB);
204 set_pwm1_duty(0);
730 cizelu 205 }
708 cizelu 206  
207 void motor_test() // test motoru
730 cizelu 208 {
708 cizelu 209 int8 i;
716 cizelu 210 beep(100,200);
708 cizelu 211 printf("TEST MOTORU\n");
212 delay_ms(1000);
716 cizelu 213 printf("LEVY MOTOR DOPREDU\n");
708 cizelu 214 for(i=0;i<255;i++)
730 cizelu 215 {
708 cizelu 216 l_motor_fwd(i);
217 printf("RYCHLOST: %u\n",i);
716 cizelu 218 delay_ms(5);
730 cizelu 219 }
708 cizelu 220 for(i=255;i>0;i--)
730 cizelu 221 {
708 cizelu 222 l_motor_fwd(i);
223 printf("RYCHLOST: %u\n",i);
716 cizelu 224 delay_ms(5);
730 cizelu 225 }
716 cizelu 226 printf("LEVY MOTOR DOZADU\n");
708 cizelu 227 for(i=0;i<255;i++)
730 cizelu 228 {
708 cizelu 229 l_motor_bwd(i);
230 printf("RYCHLOST: %u\n",i);
716 cizelu 231 delay_ms(5);
730 cizelu 232 }
708 cizelu 233 for(i=255;i>0;i--)
730 cizelu 234 {
708 cizelu 235 l_motor_bwd(i);
236 printf("RYCHLOST: %u\n",i);
716 cizelu 237 delay_ms(5);
730 cizelu 238 }
716 cizelu 239 printf("PRAVY MOTOR DOPREDU\n");
708 cizelu 240 for(i=0;i<255;i++)
730 cizelu 241 {
708 cizelu 242 r_motor_fwd(i);
716 cizelu 243 printf("RYCHLOST: %u\n",i);
244 delay_ms(5);
730 cizelu 245 }
708 cizelu 246 for(i=255;i>0;i--)
730 cizelu 247 {
708 cizelu 248 r_motor_fwd(i);
249 printf("RYCHLOST: %u\n",i);
716 cizelu 250 delay_ms(5);
730 cizelu 251 }
716 cizelu 252 printf("PRAVY MOTOR DOZADU\n");
708 cizelu 253 for(i=0;i<255;i++)
730 cizelu 254 {
708 cizelu 255 r_motor_bwd(i);
256 printf("RYCHLOST: %u\n",i);
716 cizelu 257 delay_ms(5);
730 cizelu 258 }
708 cizelu 259 for(i=255;i>0;i--)
730 cizelu 260 {
708 cizelu 261 r_motor_bwd(i);
262 printf("RYCHLOST: %u\n",i);
716 cizelu 263 delay_ms(5);
730 cizelu 264 }
265 printf("KONEC TESTU MOTORU \N");
708 cizelu 266 }
267  
268 void diagnostika()
269 {
270 read_blue_sensors();
271 printf("LEVA: %u \t",line_l);
789 cizelu 272 delay_ms(10);
708 cizelu 273 printf("PRAVA: %u \t",line_r);
789 cizelu 274 delay_ms(10);
708 cizelu 275 printf("L_NARAZ: %u \t",BUMPL);
789 cizelu 276 delay_ms(10);
708 cizelu 277 printf("P_NARAZ: %u \n",BUMPR);
789 cizelu 278 delay_ms(10);
708 cizelu 279 if(BUMPL&&BUMPR)
280 {
281 motor_test();
282 }
283 }
284  
700 cizelu 285 // HLAVNI SMYCKA
286 void main()
730 cizelu 287 {
771 kaklik 288  
708 cizelu 289 printf("POWER ON \n");
700 cizelu 290 // NASTAVENI > provede se pouze pri zapnuti
708 cizelu 291 setup_adc_ports(sAN0-sAN1-sAN2);
789 cizelu 292 setup_adc(ADC_CLOCK_INTERNAL); // interni hodniny pro AD prevodnik
700 cizelu 293 setup_spi(SPI_SS_DISABLED);
294 setup_timer_0(RTCC_INTERNAL|RTCC_DIV_1);
295 setup_timer_1(T1_DISABLED);
789 cizelu 296 setup_timer_2(T2_DIV_BY_16,255,1); // casovac pro PWM
297 setup_ccp1(CCP_PWM); // povoli PWM na pinu RC2
298 setup_ccp2(CCP_PWM); // povolí PWM na pinu RC1
700 cizelu 299 setup_comparator(NC_NC_NC_NC);
708 cizelu 300 setup_vref(FALSE);
789 cizelu 301 l_motor_off(); // vypne levy motor
302 r_motor_off(); // vypne pravy motor
303 olsa_reset(); // reset logiky radkoveho senzoru
304 olsa_setup(); // nastaveni segmentu radkoveho senzoru (offset a zisk)
305 output_high(LED1); // zhasne LED1
306 output_high(LED2); // zhasne LED2
307 beep(500,200); // pipni pri startu
708 cizelu 308 printf("OK! \n");
716 cizelu 309 delay_ms(500);
708 cizelu 310 printf("VYBRAT MOD... \n");
789 cizelu 311  
700 cizelu 312 while(true)
313 {
314 }
730 cizelu 315 }