Rev 789 Rev 790
Line 34... Line 34...
34 int start_int[8]={0,0,0,1,0,0,0,0}; // zacatek integrace 0x08 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 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 36 int readout[8]={0,1,0,0,0,0,0,0}; // cteni senzoru 0x02
37   37  
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 - 102) 39 int olsa_rseg[51]={0}; // prava cast radky (pixely 51 - 101)
40 int8 *line_lp=&olsa_lseg; // ukazatel na levou cast radky 40 int8 *line_lp=&olsa_lseg; // ukazatel na levou cast radky
41 int8 *line_rp=&olsa_rseg; // ukazatel na pravou cast radky 41 int8 *line_rp=&olsa_rseg; // ukazatel na pravou cast radky
42   -  
-   42 int8 pixel; // dec hodnota jednoho pixelu
-   43 int8 pixel_count; // cislo pixelu 0 - 101
43   44  
44 //naraznik 45 //naraznik
45 #define BUMPL input(PIN_D6) 46 #define BUMPL input(PIN_D6)
46 #define BUMPR input(PIN_D7) 47 #define BUMPR input(PIN_D7)
47   48  
Line 57... Line 58...
57 #define LMF PIN_D0 58 #define LMF PIN_D0
58 #define LMB PIN_D1 59 #define LMB PIN_D1
59 #define RMF PIN_D2 60 #define RMF PIN_D2
60 #define RMB PIN_D3 61 #define RMB PIN_D3
61   62  
-   63 int8 lm_speed;
-   64 int8 rm_speed;
-   65  
62 //PODPROGRAMY 66 //PODPROGRAMY
63 //SENZORY 67 //SENZORY
64 //OLSA01A 68 //OLSA01A
65 void olsa_pulses(int count) // vytvori impulzy pro ridici logiku 69 void olsa_pulses(int count) // vytvori impulzy pro ridici logiku
66 { 70 {
Line 82... Line 86...
82 output_low(SCLK); 86 output_low(SCLK);
83 } 87 }
84   88  
85 void olsa_send(int info[]) // USART komunikace s modulem OLSA01A - poslani zpravy 89 void olsa_send(int info[]) // USART komunikace s modulem OLSA01A - poslani zpravy
86 { 90 {
87 int8 *ip=&info; // ukazatel na pole s informaci 91 int8 *ip=&info; // ukazatel na pole s informaci
88 int i; // pomocna promenna pro nastaveni 0 nebo 1 na SDIN 92 int i; // pomocna promenna pro nastaveni 0 nebo 1 na SDIN
89 93
90 output_low(SDIN); // start bit 94 output_low(SDIN); // start bit
91 olsa_pulse(); 95 olsa_pulse();
92 for(ip=0;ip<8;ip++) // predani informace - 8 bit, LSB prvni > MSB posledni 96 for(ip=0;ip<8;ip++) // predani informace - 8 bit, LSB prvni > MSB posledni
93 { 97 {
Line 132... Line 136...
132 olsa_send(right_offset); // nastaveni praveho segmentu (offset a zisk) 136 olsa_send(right_offset); // nastaveni praveho segmentu (offset a zisk)
133 olsa_send(offset); 137 olsa_send(offset);
134 olsa_send(right_gain); 138 olsa_send(right_gain);
135 olsa_send(gain); 139 olsa_send(gain);
136 } 140 }
-   141
-   142 void olsa_integration() // snimani pixelu
-   143 {
-   144 olsa_send(start_int); // zacatek integrace senzoru
-   145 olsa_pulses(22);
-   146 olsa_send(stop_int); // konec integrace senzoru
-   147 olsa_pulses(5);
-   148 }
137   149  
138 //ZACHRANNE SENZORY 150 //ZACHRANNE SENZORY
139 void read_blue_sensors() // cteni nouzovych senzoru 151 void read_blue_sensors() // cteni nouzovych senzoru
140 { 152 {
141 set_adc_channel(LINEL); // cti levy nouzovy senzor 153 set_adc_channel(LINEL); // cti levy nouzovy senzor
142 delay_us(10); 154 delay_us(10);
143 line_l=read_adc(); 155 line_l=read_adc();
144 set_adc_channel(LINER); // cti pravy nouzovy senzor 156 set_adc_channel(LINER); // cti pravy nouzovy senzor
145 delay_us(10); 157 delay_us(10);
146 line_r=read_adc(); 158 line_r=read_adc();
147 } 159 }
148 160
149 //PIPAK 161 //PIPAK
150 void beep(int16 period,int16 length) 162 void beep(int16 period,int16 length)
151 { 163 {
152 int16 bp; //promenna pro nastaveni delky 164 int16 bp; //promenna pro nastaveni delky
153 165
154 for(bp=length;bp>0;bp--) 166 for(bp=length;bp>0;bp--)
155 { 167 {
156 output_high(SOUND_HI); 168 output_high(SOUND_HI);
157 output_low(SOUND_LO); 169 output_low(SOUND_LO);
Line 160... Line 172...
160 output_low(SOUND_HI); 172 output_low(SOUND_HI);
161 delay_us(period); 173 delay_us(period);
162 } 174 }
163 } 175 }
164 //MOTORY 176 //MOTORY
165 void l_motor_fwd(int8 speedl) // levy motor dopredu 177 void l_motor_fwd(int8 speedl) // levy motor dopredu
166 { 178 {
167 output_high(LMF); 179 output_high(LMF);
168 output_low(LMB); 180 output_low(LMB);
169 set_pwm2_duty(speedl); 181 set_pwm2_duty(speedl);
170 } 182 }
171   183  
172 void l_motor_bwd(int8 speedl) // levy motor dozadu 184 void l_motor_bwd(int8 speedl) // levy motor dozadu
173 { 185 {
174 output_high(LMB); 186 output_high(LMB);
175 output_low(LMF); 187 output_low(LMF);
176 set_pwm2_duty(speedl); 188 set_pwm2_duty(speedl);
177 } 189 }
178   190  
179 void r_motor_fwd(int8 speedr) // pravy motor dopredu 191 void r_motor_fwd(int8 speedr) // pravy motor dopredu
180 { 192 {
181 output_high(RMF); 193 output_high(RMF);
182 output_low(RMB); 194 output_low(RMB);
183 set_pwm1_duty(speedr); 195 set_pwm1_duty(speedr);
184 } 196 }
185   197  
186 void r_motor_bwd(int8 speedr) // pravy motor dozadu 198 void r_motor_bwd(int8 speedr) // pravy motor dozadu
187 { 199 {
188 output_high(RMB); 200 output_high(RMB);
189 output_low(RMF); 201 output_low(RMF);
190 set_pwm1_duty(speedr); 202 set_pwm1_duty(speedr);
191 } 203 }
192   204  
193 void l_motor_off() // levy motor vypnut 205 void l_motor_off() // levy motor vypnut
194 { 206 {
195 output_low(LMF); 207 output_low(LMF);
196 output_low(LMB); 208 output_low(LMB);
197 set_pwm2_duty(0); 209 set_pwm2_duty(0);
198 } 210 }
199 211
200 void r_motor_off() // pravy motor vypnut 212 void r_motor_off() // pravy motor vypnut
201 { 213 {
202 output_low(RMF); 214 output_low(RMF);
203 output_low(RMB); 215 output_low(RMB);
204 set_pwm1_duty(0); 216 set_pwm1_duty(0);
205 } 217 }
206   218  
207 void motor_test() // test motoru 219 void motor_test() // test motoru
208 { 220 {
209 int8 i; 221 int8 i;
210 beep(100,200); 222 beep(100,200);
211 printf("TEST MOTORU\n"); 223 printf("TEST MOTORU\n");
212 delay_ms(1000); 224 delay_ms(1000);
Line 263... Line 275...
263 delay_ms(5); 275 delay_ms(5);
264 } 276 }
265 printf("KONEC TESTU MOTORU \N"); 277 printf("KONEC TESTU MOTORU \N");
266 } 278 }
267   279  
268 void diagnostika() 280 void diagnostika() // diagnostika - vypis senzoru s moznosti prepnuti na test motoru
269 { 281 {
270 read_blue_sensors(); 282 read_blue_sensors();
271 printf("LEVA: %u \t",line_l); 283 printf("LEVA: %u \t",line_l);
272 delay_ms(10); 284 delay_ms(10);
273 printf("PRAVA: %u \t",line_r); 285 printf("PRAVA: %u \t",line_r);
274 delay_ms(10); 286 delay_ms(10);
275 printf("L_NARAZ: %u \t",BUMPL); 287 printf("L_NARAZ: %u \t",BUMPL);
276 delay_ms(10); 288 delay_ms(10);
277 printf("P_NARAZ: %u \n",BUMPR); 289 printf("P_NARAZ: %u \n",BUMPR);
278 delay_ms(10); 290 delay_ms(10);
279 if(BUMPL&&BUMPR) 291 if(BUMPL&&BUMPR) // po zmacknuti stran narazniku spusti test motoru
280 { 292 {
281 motor_test(); 293 motor_test();
282 } 294 }
283 } 295 }
284   296