| 2 |
kaklik |
1 |
CCS PCM C Compiler, Version 3.245, 27853 15-IV-06 16:28
|
|
|
2 |
|
|
|
3 |
Filename: D:\KAKLIK\programy\PIC_C\roboti\istrobot\3Orbis\main.lst
|
|
|
4 |
|
|
|
5 |
ROM used: 598 words (15%)
|
|
|
6 |
Largest free fragment is 2048
|
|
|
7 |
RAM used: 31 (18%) at main() level
|
|
|
8 |
40 (23%) worst case
|
|
|
9 |
Stack: 4 worst case (2 in main + 2 for interrupts)
|
|
|
10 |
|
|
|
11 |
*
|
|
|
12 |
0000: MOVLW 00
|
|
|
13 |
0001: MOVWF 0A
|
|
|
14 |
0002: GOTO 182
|
|
|
15 |
0003: NOP
|
|
|
16 |
.................... #include ".\main.h"
|
|
|
17 |
.................... #include <16F88.h>
|
|
|
18 |
.................... //////// Standard Header file for the PIC16F88 device ////////////////
|
|
|
19 |
.................... #device PIC16F88
|
|
|
20 |
.................... #list
|
|
|
21 |
....................
|
|
|
22 |
.................... #device adc=8
|
|
|
23 |
....................
|
|
|
24 |
.................... #FUSES NOWDT //No Watch Dog Timer
|
|
|
25 |
.................... #FUSES INTRC_IO
|
|
|
26 |
.................... #FUSES NOPUT //No Power Up Timer
|
|
|
27 |
.................... #FUSES MCLR //Master Clear pin enabled
|
|
|
28 |
.................... #FUSES NOBROWNOUT //Reset when brownout detected
|
|
|
29 |
.................... #FUSES NOLVP //Low Voltage Programming on B3(PIC16) or B5(PIC18)
|
|
|
30 |
.................... #FUSES NOCPD //No EE protection
|
|
|
31 |
.................... #FUSES NOWRT //Program memory not write protected
|
|
|
32 |
.................... #FUSES NODEBUG //No Debug mode for ICD
|
|
|
33 |
.................... #FUSES NOPROTECT //Code not protected from reading
|
|
|
34 |
.................... #FUSES NOFCMEN //Fail-safe clock monitor enabled
|
|
|
35 |
.................... #FUSES NOIESO //Internal External Switch Over mode enabled
|
|
|
36 |
....................
|
|
|
37 |
.................... #use delay(clock=8000000,RESTART_WDT)
|
|
|
38 |
000C: MOVLW 3D
|
|
|
39 |
000D: MOVWF 04
|
|
|
40 |
000E: MOVF 00,W
|
|
|
41 |
000F: BTFSC 03.2
|
|
|
42 |
0010: GOTO 023
|
|
|
43 |
0011: MOVLW 02
|
|
|
44 |
0012: MOVWF 78
|
|
|
45 |
0013: MOVLW BF
|
|
|
46 |
0014: MOVWF 77
|
|
|
47 |
0015: CLRWDT
|
|
|
48 |
0016: DECFSZ 77,F
|
|
|
49 |
0017: GOTO 015
|
|
|
50 |
0018: DECFSZ 78,F
|
|
|
51 |
0019: GOTO 013
|
|
|
52 |
001A: MOVLW 96
|
|
|
53 |
001B: MOVWF 77
|
|
|
54 |
001C: DECFSZ 77,F
|
|
|
55 |
001D: GOTO 01C
|
|
|
56 |
001E: NOP
|
|
|
57 |
001F: NOP
|
|
|
58 |
0020: CLRWDT
|
|
|
59 |
0021: DECFSZ 00,F
|
|
|
60 |
0022: GOTO 011
|
|
|
61 |
0023: RETLW 00
|
|
|
62 |
0024: MOVLW 08
|
|
|
63 |
0025: SUBWF 42,F
|
|
|
64 |
0026: BTFSS 03.0
|
|
|
65 |
0027: GOTO 033
|
|
|
66 |
0028: MOVLW 42
|
|
|
67 |
0029: MOVWF 04
|
|
|
68 |
002A: BCF 03.0
|
|
|
69 |
002B: RRF 00,F
|
|
|
70 |
002C: MOVF 00,W
|
|
|
71 |
002D: BTFSC 03.2
|
|
|
72 |
002E: GOTO 033
|
|
|
73 |
002F: GOTO 031
|
|
|
74 |
0030: CLRWDT
|
|
|
75 |
0031: DECFSZ 00,F
|
|
|
76 |
0032: GOTO 030
|
|
|
77 |
0033: RETLW 00
|
|
|
78 |
....................
|
|
|
79 |
....................
|
|
|
80 |
....................
|
|
|
81 |
.................... #use rtos (timer=2, minor_cycle=2ms)
|
|
|
82 |
....................
|
|
|
83 |
.................... #define STRED 128 // sredni poloha zadniho kolecka
|
|
|
84 |
.................... #define BEAR1 12 // 3 stupne zataceni
|
|
|
85 |
.................... #define BEAR2 34
|
|
|
86 |
.................... #define BEAR3 70
|
|
|
87 |
.................... #define SPEEDMAX 120 // maximalni rychlost
|
|
|
88 |
....................
|
|
|
89 |
.................... #define L 1
|
|
|
90 |
.................... #define S 2
|
|
|
91 |
.................... #define R 0
|
|
|
92 |
....................
|
|
|
93 |
.................... // servo
|
|
|
94 |
.................... #define SERVO PIN_A2
|
|
|
95 |
....................
|
|
|
96 |
.................... // IR
|
|
|
97 |
.................... #define IRTX PIN_B2
|
|
|
98 |
....................
|
|
|
99 |
.................... //motory
|
|
|
100 |
.................... #define FR output_low(PIN_A7); output_high(PIN_A6) // Vpred
|
|
|
101 |
.................... #define FL output_low(PIN_A1); output_high(PIN_A0)
|
|
|
102 |
.................... #define BR output_low(PIN_A6); output_high(PIN_A7) // Vzad
|
|
|
103 |
.................... #define BL output_low(PIN_A0); output_high(PIN_A1)
|
|
|
104 |
.................... #define STOPR output_low(PIN_A6);output_low(PIN_A7)
|
|
|
105 |
.................... #define STOPL output_low(PIN_A0);output_low(PIN_A1)
|
|
|
106 |
....................
|
|
|
107 |
.................... //HID
|
|
|
108 |
.................... #define LED1 PIN_B1 //oranzova
|
|
|
109 |
.................... #define LED2 PIN_B2 //zluta
|
|
|
110 |
....................
|
|
|
111 |
.................... #define STROBE PIN_B0
|
|
|
112 |
.................... //#define SW1 PIN_A2 // Motory On/off
|
|
|
113 |
....................
|
|
|
114 |
.................... unsigned int8 sensors; // pomocna promenna pro cteni cidel na caru
|
|
|
115 |
.................... unsigned int8 line; // na ktere strane byla detekovana cara
|
|
|
116 |
.................... //unsigned int8 dira; // pocita dobu po kterou je ztracena cara
|
|
|
117 |
.................... unsigned int8 uhel; // urcuje aktualni uhel zataceni
|
|
|
118 |
.................... //unsigned int8 speed; // maximalni povolena rychlost
|
|
|
119 |
.................... unsigned int8 rovinka; // pocitadlo na zjisteni rovinky
|
|
|
120 |
....................
|
|
|
121 |
.................... signed int16 Lmotor; // promene, ktere urcuji velikost vykonu na levem
|
|
|
122 |
.................... signed int16 Rmotor; // a pravem motoru
|
|
|
123 |
....................
|
|
|
124 |
.................... // makro pro PWM
|
|
|
125 |
.................... #define GO(motor, direction, power) if(get_timer0()<=power) \
|
|
|
126 |
.................... {direction##motor;} else {stop##motor;}
|
|
|
127 |
....................
|
|
|
128 |
.................... ////////////////////////////////////////////////////////////////////////////////
|
|
|
129 |
.................... ////////////////////////////////////////////////////////////////////////////////
|
|
|
130 |
.................... /*
|
|
|
131 |
.................... void diagnostika()
|
|
|
132 |
.................... {
|
|
|
133 |
.................... if(input(SW1))STOPR;STOPL;While(TRUE);
|
|
|
134 |
.................... // if(LSENSOR==true) output_high(LED2); else output_low(LED2);
|
|
|
135 |
.................... // if(RSENSOR==true) output_high(LED1); else output_low(LED1);
|
|
|
136 |
.................... }
|
|
|
137 |
.................... */
|
|
|
138 |
.................... ////////////////////////////////////////////////////////////////////////////////
|
|
|
139 |
.................... #task (rate=18ms,max=2ms)
|
|
|
140 |
.................... void zatoc() // ridi servo ktere otaci kolem
|
|
|
141 |
.................... {
|
|
|
142 |
.................... unsigned int8 n;
|
|
|
143 |
....................
|
|
|
144 |
.................... output_high(SERVO);
|
|
|
145 |
0034: BSF 03.5
|
|
|
146 |
0035: BCF 05.2
|
|
|
147 |
0036: BCF 03.5
|
|
|
148 |
0037: BSF 05.2
|
|
|
149 |
.................... delay_us(1100);
|
|
|
150 |
0038: CLRWDT
|
|
|
151 |
0039: MOVLW 01
|
|
|
152 |
003A: MOVWF 3D
|
|
|
153 |
003B: CALL 00C
|
|
|
154 |
003C: MOVLW 63
|
|
|
155 |
003D: MOVWF 42
|
|
|
156 |
003E: CALL 024
|
|
|
157 |
.................... for(n=uhel; n>0; n--);
|
|
|
158 |
003F: MOVF 23,W
|
|
|
159 |
0040: MOVWF 3C
|
|
|
160 |
0041: MOVF 3C,F
|
|
|
161 |
0042: BTFSC 03.2
|
|
|
162 |
0043: GOTO 046
|
|
|
163 |
0044: DECF 3C,F
|
|
|
164 |
0045: GOTO 041
|
|
|
165 |
.................... output_low(SERVO);
|
|
|
166 |
0046: BSF 03.5
|
|
|
167 |
0047: BCF 05.2
|
|
|
168 |
0048: BCF 03.5
|
|
|
169 |
0049: BCF 05.2
|
|
|
170 |
.................... }
|
|
|
171 |
004A: MOVLW 34
|
|
|
172 |
004B: MOVWF 2E
|
|
|
173 |
004C: MOVLW 00
|
|
|
174 |
004D: MOVWF 2F
|
|
|
175 |
004E: BCF 0A.3
|
|
|
176 |
004F: GOTO 234
|
|
|
177 |
0050: RETLW 00
|
|
|
178 |
....................
|
|
|
179 |
.................... ////////////////////////////////////////////////////////////////////////////////
|
|
|
180 |
.................... void diag() // ridi servo ktere otaci kolem
|
|
|
181 |
.................... {
|
|
|
182 |
.................... unsigned int8 n;
|
|
|
183 |
....................
|
|
|
184 |
.................... output_high(SERVO);
|
|
|
185 |
*
|
|
|
186 |
016A: BSF 03.5
|
|
|
187 |
016B: BCF 05.2
|
|
|
188 |
016C: BCF 03.5
|
|
|
189 |
016D: BSF 05.2
|
|
|
190 |
.................... delay_us(1100);
|
|
|
191 |
016E: CLRWDT
|
|
|
192 |
016F: MOVLW 01
|
|
|
193 |
0170: MOVWF 3D
|
|
|
194 |
0171: CALL 00C
|
|
|
195 |
0172: MOVLW 63
|
|
|
196 |
0173: MOVWF 42
|
|
|
197 |
0174: CALL 024
|
|
|
198 |
.................... for(n=uhel; n>0; n--);
|
|
|
199 |
0175: MOVF 23,W
|
|
|
200 |
0176: MOVWF 3A
|
|
|
201 |
0177: MOVF 3A,F
|
|
|
202 |
0178: BTFSC 03.2
|
|
|
203 |
0179: GOTO 17C
|
|
|
204 |
017A: DECF 3A,F
|
|
|
205 |
017B: GOTO 177
|
|
|
206 |
.................... output_low(SERVO);
|
|
|
207 |
017C: BSF 03.5
|
|
|
208 |
017D: BCF 05.2
|
|
|
209 |
017E: BCF 03.5
|
|
|
210 |
017F: BCF 05.2
|
|
|
211 |
.................... }
|
|
|
212 |
0180: BCF 0A.3
|
|
|
213 |
0181: GOTO 1D4 (RETURN)
|
|
|
214 |
....................
|
|
|
215 |
.................... ////////////////////////////////////////////////////////////////////////////////
|
|
|
216 |
.................... short int IRcheck() // potvrdi detekci cihly
|
|
|
217 |
.................... {
|
|
|
218 |
.................... output_high(IRTX); // vypne vysilac IR
|
|
|
219 |
....................
|
|
|
220 |
.................... output_low(STROBE);
|
|
|
221 |
.................... sensors = spi_read(0); // cteni senzoru
|
|
|
222 |
.................... output_high(STROBE);
|
|
|
223 |
....................
|
|
|
224 |
.................... if(true==bit_test(sensors,7)) // otestuje, jestli je stale detekovan IR signal
|
|
|
225 |
.................... {
|
|
|
226 |
.................... output_low(IRTX); // zapne vysilac IR
|
|
|
227 |
....................
|
|
|
228 |
.................... output_low(STROBE);
|
|
|
229 |
.................... sensors = spi_read(0); // cteni senzoru
|
|
|
230 |
.................... output_high(STROBE);
|
|
|
231 |
....................
|
|
|
232 |
.................... if(false==bit_test(sensors,7)) // otestuje, jestli je detekovana cihla
|
|
|
233 |
.................... {
|
|
|
234 |
.................... output_high(IRTX); // vypne vysilac IR
|
|
|
235 |
....................
|
|
|
236 |
.................... output_low(STROBE);
|
|
|
237 |
.................... sensors = spi_read(0); // cteni senzoru
|
|
|
238 |
.................... output_high(STROBE);
|
|
|
239 |
....................
|
|
|
240 |
.................... if(bit_test(sensors,7)) return 1; //
|
|
|
241 |
....................
|
|
|
242 |
.................... else return 0; // vrat 0 kdyz je ruseni
|
|
|
243 |
.................... }
|
|
|
244 |
.................... else return 0; // vrat 0 kdyz to nebyla cihla
|
|
|
245 |
.................... }
|
|
|
246 |
.................... else return 0; // vrat 0 kdyz je detekovano ruseni
|
|
|
247 |
.................... }
|
|
|
248 |
.................... ////////////////////////////////////////////////////////////////////////////////
|
|
|
249 |
.................... /*
|
|
|
250 |
.................... #task (rate=4ms, max=20us)
|
|
|
251 |
.................... void rychlost()
|
|
|
252 |
.................... {
|
|
|
253 |
.................... if(speed<255) speed++;
|
|
|
254 |
.................... }
|
|
|
255 |
.................... */
|
|
|
256 |
.................... ////////////////////////////////////////////////////////////////////////////////
|
|
|
257 |
.................... #task (rate=2ms, max=1ms)
|
|
|
258 |
.................... void rizeni()
|
|
|
259 |
.................... {
|
|
|
260 |
.................... unsigned int16 n;
|
|
|
261 |
.................... unsigned int8 i;
|
|
|
262 |
....................
|
|
|
263 |
.................... GO(L,F,Lmotor);GO(R,F,Rmotor); // zapni motory
|
|
|
264 |
*
|
|
|
265 |
0051: MOVF 01,W
|
|
|
266 |
0052: BTFSS 00.7
|
|
|
267 |
0053: GOTO 057
|
|
|
268 |
0054: BTFSS 26.7
|
|
|
269 |
0055: GOTO 05F
|
|
|
270 |
0056: GOTO 059
|
|
|
271 |
0057: BTFSC 26.7
|
|
|
272 |
0058: GOTO 068
|
|
|
273 |
0059: MOVF 26,F
|
|
|
274 |
005A: BTFSS 03.2
|
|
|
275 |
005B: GOTO 05F
|
|
|
276 |
005C: SUBWF 25,W
|
|
|
277 |
005D: BTFSS 03.0
|
|
|
278 |
005E: GOTO 068
|
|
|
279 |
005F: BSF 03.5
|
|
|
280 |
0060: BCF 05.1
|
|
|
281 |
0061: BCF 03.5
|
|
|
282 |
0062: BCF 05.1
|
|
|
283 |
0063: BSF 03.5
|
|
|
284 |
0064: BCF 05.0
|
|
|
285 |
0065: BCF 03.5
|
|
|
286 |
0066: BSF 05.0
|
|
|
287 |
0067: GOTO 070
|
|
|
288 |
0068: BSF 03.5
|
|
|
289 |
0069: BCF 05.0
|
|
|
290 |
006A: BCF 03.5
|
|
|
291 |
006B: BCF 05.0
|
|
|
292 |
006C: BSF 03.5
|
|
|
293 |
006D: BCF 05.1
|
|
|
294 |
006E: BCF 03.5
|
|
|
295 |
006F: BCF 05.1
|
|
|
296 |
0070: MOVF 01,W
|
|
|
297 |
0071: BTFSS 00.7
|
|
|
298 |
0072: GOTO 076
|
|
|
299 |
0073: BTFSS 28.7
|
|
|
300 |
0074: GOTO 07E
|
|
|
301 |
0075: GOTO 078
|
|
|
302 |
0076: BTFSC 28.7
|
|
|
303 |
0077: GOTO 087
|
|
|
304 |
0078: MOVF 28,F
|
|
|
305 |
0079: BTFSS 03.2
|
|
|
306 |
007A: GOTO 07E
|
|
|
307 |
007B: SUBWF 27,W
|
|
|
308 |
007C: BTFSS 03.0
|
|
|
309 |
007D: GOTO 087
|
|
|
310 |
007E: BSF 03.5
|
|
|
311 |
007F: BCF 05.7
|
|
|
312 |
0080: BCF 03.5
|
|
|
313 |
0081: BCF 05.7
|
|
|
314 |
0082: BSF 03.5
|
|
|
315 |
0083: BCF 05.6
|
|
|
316 |
0084: BCF 03.5
|
|
|
317 |
0085: BSF 05.6
|
|
|
318 |
0086: GOTO 08F
|
|
|
319 |
0087: BSF 03.5
|
|
|
320 |
0088: BCF 05.6
|
|
|
321 |
0089: BCF 03.5
|
|
|
322 |
008A: BCF 05.6
|
|
|
323 |
008B: BSF 03.5
|
|
|
324 |
008C: BCF 05.7
|
|
|
325 |
008D: BCF 03.5
|
|
|
326 |
008E: BCF 05.7
|
|
|
327 |
....................
|
|
|
328 |
.................... delay_us(500);
|
|
|
329 |
008F: CLRWDT
|
|
|
330 |
0090: MOVLW 02
|
|
|
331 |
0091: MOVWF 41
|
|
|
332 |
0092: MOVLW F7
|
|
|
333 |
0093: MOVWF 42
|
|
|
334 |
0094: CALL 024
|
|
|
335 |
0095: DECFSZ 41,F
|
|
|
336 |
0096: GOTO 092
|
|
|
337 |
....................
|
|
|
338 |
.................... output_low(STROBE);
|
|
|
339 |
0097: BSF 03.5
|
|
|
340 |
0098: BCF 06.0
|
|
|
341 |
0099: BCF 03.5
|
|
|
342 |
009A: BCF 06.0
|
|
|
343 |
.................... sensors = spi_read(0); // cteni senzoru
|
|
|
344 |
009B: MOVF 13,W
|
|
|
345 |
009C: CLRF 13
|
|
|
346 |
009D: BSF 03.5
|
|
|
347 |
009E: BTFSC 14.0
|
|
|
348 |
009F: GOTO 0A2
|
|
|
349 |
00A0: BCF 03.5
|
|
|
350 |
00A1: GOTO 09D
|
|
|
351 |
00A2: BCF 03.5
|
|
|
352 |
00A3: MOVF 13,W
|
|
|
353 |
00A4: MOVWF 21
|
|
|
354 |
.................... sensors=~sensors;
|
|
|
355 |
00A5: COMF 21,F
|
|
|
356 |
.................... output_high(STROBE);
|
|
|
357 |
00A6: BSF 03.5
|
|
|
358 |
00A7: BCF 06.0
|
|
|
359 |
00A8: BCF 03.5
|
|
|
360 |
00A9: BSF 06.0
|
|
|
361 |
....................
|
|
|
362 |
.................... i=0; // havarijni kod
|
|
|
363 |
00AA: CLRF 40
|
|
|
364 |
.................... for (n=0; n<=6; n++)
|
|
|
365 |
00AB: CLRF 3F
|
|
|
366 |
00AC: CLRF 3E
|
|
|
367 |
00AD: MOVF 3F,F
|
|
|
368 |
00AE: BTFSS 03.2
|
|
|
369 |
00AF: GOTO 0C4
|
|
|
370 |
00B0: MOVF 3E,W
|
|
|
371 |
00B1: SUBLW 06
|
|
|
372 |
00B2: BTFSS 03.0
|
|
|
373 |
00B3: GOTO 0C4
|
|
|
374 |
.................... {
|
|
|
375 |
.................... if(bit_test(sensors,n)) i++;
|
|
|
376 |
00B4: MOVF 21,W
|
|
|
377 |
00B5: MOVWF 77
|
|
|
378 |
00B6: MOVF 3E,W
|
|
|
379 |
00B7: MOVWF 78
|
|
|
380 |
00B8: BTFSC 03.2
|
|
|
381 |
00B9: GOTO 0BE
|
|
|
382 |
00BA: BCF 03.0
|
|
|
383 |
00BB: RRF 77,F
|
|
|
384 |
00BC: DECFSZ 78,F
|
|
|
385 |
00BD: GOTO 0BA
|
|
|
386 |
00BE: BTFSC 77.0
|
|
|
387 |
00BF: INCF 40,F
|
|
|
388 |
.................... }
|
|
|
389 |
00C0: INCF 3E,F
|
|
|
390 |
00C1: BTFSC 03.2
|
|
|
391 |
00C2: INCF 3F,F
|
|
|
392 |
00C3: GOTO 0AD
|
|
|
393 |
.................... if (i>3) rtos_terminate(); // zastavi, kdyz je cerno pod vice nez tremi cidly
|
|
|
394 |
00C4: MOVF 40,W
|
|
|
395 |
00C5: SUBLW 03
|
|
|
396 |
00C6: BTFSS 03.0
|
|
|
397 |
00C7: GOTO 241
|
|
|
398 |
....................
|
|
|
399 |
....................
|
|
|
400 |
.................... if(bit_test(sensors,3)) //...|...//
|
|
|
401 |
00C8: BTFSS 21.3
|
|
|
402 |
00C9: GOTO 0D8
|
|
|
403 |
.................... {
|
|
|
404 |
.................... uhel=STRED;
|
|
|
405 |
00CA: MOVLW 80
|
|
|
406 |
00CB: MOVWF 23
|
|
|
407 |
.................... Lmotor=SPEEDMAX;
|
|
|
408 |
00CC: CLRF 26
|
|
|
409 |
00CD: MOVLW 78
|
|
|
410 |
00CE: MOVWF 25
|
|
|
411 |
.................... Rmotor=SPEEDMAX;
|
|
|
412 |
00CF: CLRF 28
|
|
|
413 |
00D0: MOVWF 27
|
|
|
414 |
.................... line=S;
|
|
|
415 |
00D1: MOVLW 02
|
|
|
416 |
00D2: MOVWF 22
|
|
|
417 |
.................... if (rovinka<255) rovinka++;
|
|
|
418 |
00D3: INCFSZ 24,W
|
|
|
419 |
00D4: GOTO 0D6
|
|
|
420 |
00D5: GOTO 0D7
|
|
|
421 |
00D6: INCF 24,F
|
|
|
422 |
.................... return;
|
|
|
423 |
00D7: GOTO 163
|
|
|
424 |
.................... }
|
|
|
425 |
....................
|
|
|
426 |
.................... if(bit_test(sensors,0)) //|......// // z duvodu zkraceni doby reakce se cidla nevyhodnocuji poporade ale od krajnich k prostrednimu
|
|
|
427 |
00D8: BTFSS 21.0
|
|
|
428 |
00D9: GOTO 0E4
|
|
|
429 |
.................... {
|
|
|
430 |
.................... uhel=STRED-BEAR3;
|
|
|
431 |
00DA: MOVLW 3A
|
|
|
432 |
00DB: MOVWF 23
|
|
|
433 |
.................... Lmotor=0;
|
|
|
434 |
00DC: CLRF 26
|
|
|
435 |
00DD: CLRF 25
|
|
|
436 |
.................... Rmotor=SPEEDMAX;
|
|
|
437 |
00DE: CLRF 28
|
|
|
438 |
00DF: MOVLW 78
|
|
|
439 |
00E0: MOVWF 27
|
|
|
440 |
.................... line=L;
|
|
|
441 |
00E1: MOVLW 01
|
|
|
442 |
00E2: MOVWF 22
|
|
|
443 |
.................... return;
|
|
|
444 |
00E3: GOTO 163
|
|
|
445 |
.................... }
|
|
|
446 |
....................
|
|
|
447 |
.................... if(bit_test(sensors,6)) //......|//
|
|
|
448 |
00E4: BTFSS 21.6
|
|
|
449 |
00E5: GOTO 0EF
|
|
|
450 |
.................... {
|
|
|
451 |
.................... uhel=STRED+BEAR3;
|
|
|
452 |
00E6: MOVLW C6
|
|
|
453 |
00E7: MOVWF 23
|
|
|
454 |
.................... Rmotor=0;
|
|
|
455 |
00E8: CLRF 28
|
|
|
456 |
00E9: CLRF 27
|
|
|
457 |
.................... Lmotor=SPEEDMAX;
|
|
|
458 |
00EA: CLRF 26
|
|
|
459 |
00EB: MOVLW 78
|
|
|
460 |
00EC: MOVWF 25
|
|
|
461 |
.................... line=R;
|
|
|
462 |
00ED: CLRF 22
|
|
|
463 |
.................... return;
|
|
|
464 |
00EE: GOTO 163
|
|
|
465 |
....................
|
|
|
466 |
.................... }
|
|
|
467 |
....................
|
|
|
468 |
.................... if(bit_test(sensors,1)) //.|.....//
|
|
|
469 |
00EF: BTFSS 21.1
|
|
|
470 |
00F0: GOTO 0FC
|
|
|
471 |
.................... {
|
|
|
472 |
.................... uhel=STRED-BEAR2;
|
|
|
473 |
00F1: MOVLW 5E
|
|
|
474 |
00F2: MOVWF 23
|
|
|
475 |
.................... Lmotor=SPEEDMAX-50;
|
|
|
476 |
00F3: CLRF 26
|
|
|
477 |
00F4: MOVLW 46
|
|
|
478 |
00F5: MOVWF 25
|
|
|
479 |
.................... Rmotor=SPEEDMAX;
|
|
|
480 |
00F6: CLRF 28
|
|
|
481 |
00F7: MOVLW 78
|
|
|
482 |
00F8: MOVWF 27
|
|
|
483 |
.................... line=S;
|
|
|
484 |
00F9: MOVLW 02
|
|
|
485 |
00FA: MOVWF 22
|
|
|
486 |
.................... return;
|
|
|
487 |
00FB: GOTO 163
|
|
|
488 |
.................... }
|
|
|
489 |
....................
|
|
|
490 |
.................... if(bit_test(sensors,5)) //.....|.//
|
|
|
491 |
00FC: BTFSS 21.5
|
|
|
492 |
00FD: GOTO 109
|
|
|
493 |
.................... {
|
|
|
494 |
.................... uhel=STRED+BEAR2;
|
|
|
495 |
00FE: MOVLW A2
|
|
|
496 |
00FF: MOVWF 23
|
|
|
497 |
.................... Rmotor=SPEEDMAX-50;
|
|
|
498 |
0100: CLRF 28
|
|
|
499 |
0101: MOVLW 46
|
|
|
500 |
0102: MOVWF 27
|
|
|
501 |
.................... Lmotor=SPEEDMAX;
|
|
|
502 |
0103: CLRF 26
|
|
|
503 |
0104: MOVLW 78
|
|
|
504 |
0105: MOVWF 25
|
|
|
505 |
.................... line=S;
|
|
|
506 |
0106: MOVLW 02
|
|
|
507 |
0107: MOVWF 22
|
|
|
508 |
.................... return;
|
|
|
509 |
0108: GOTO 163
|
|
|
510 |
.................... }
|
|
|
511 |
....................
|
|
|
512 |
.................... if (bit_test(sensors,2)) //..|....//
|
|
|
513 |
0109: BTFSS 21.2
|
|
|
514 |
010A: GOTO 119
|
|
|
515 |
.................... {
|
|
|
516 |
.................... uhel=STRED-BEAR1;
|
|
|
517 |
010B: MOVLW 74
|
|
|
518 |
010C: MOVWF 23
|
|
|
519 |
.................... Lmotor=SPEEDMAX;
|
|
|
520 |
010D: CLRF 26
|
|
|
521 |
010E: MOVLW 78
|
|
|
522 |
010F: MOVWF 25
|
|
|
523 |
.................... Rmotor=SPEEDMAX;
|
|
|
524 |
0110: CLRF 28
|
|
|
525 |
0111: MOVWF 27
|
|
|
526 |
.................... line=S;
|
|
|
527 |
0112: MOVLW 02
|
|
|
528 |
0113: MOVWF 22
|
|
|
529 |
.................... if (rovinka<255) rovinka++;
|
|
|
530 |
0114: INCFSZ 24,W
|
|
|
531 |
0115: GOTO 117
|
|
|
532 |
0116: GOTO 118
|
|
|
533 |
0117: INCF 24,F
|
|
|
534 |
.................... return;
|
|
|
535 |
0118: GOTO 163
|
|
|
536 |
.................... }
|
|
|
537 |
....................
|
|
|
538 |
.................... if (bit_test(sensors,4)) //....|..//
|
|
|
539 |
0119: BTFSS 21.4
|
|
|
540 |
011A: GOTO 129
|
|
|
541 |
.................... {
|
|
|
542 |
.................... uhel=STRED+BEAR1;
|
|
|
543 |
011B: MOVLW 8C
|
|
|
544 |
011C: MOVWF 23
|
|
|
545 |
.................... Rmotor=SPEEDMAX;
|
|
|
546 |
011D: CLRF 28
|
|
|
547 |
011E: MOVLW 78
|
|
|
548 |
011F: MOVWF 27
|
|
|
549 |
.................... Lmotor=SPEEDMAX;
|
|
|
550 |
0120: CLRF 26
|
|
|
551 |
0121: MOVWF 25
|
|
|
552 |
.................... line=S;
|
|
|
553 |
0122: MOVLW 02
|
|
|
554 |
0123: MOVWF 22
|
|
|
555 |
.................... if (rovinka<255) rovinka++;
|
|
|
556 |
0124: INCFSZ 24,W
|
|
|
557 |
0125: GOTO 127
|
|
|
558 |
0126: GOTO 128
|
|
|
559 |
0127: INCF 24,F
|
|
|
560 |
.................... return;
|
|
|
561 |
0128: GOTO 163
|
|
|
562 |
.................... }
|
|
|
563 |
....................
|
|
|
564 |
.................... if ((L==line) || (R==line)) // Brzdeni pri vyjeti z trate
|
|
|
565 |
0129: DECFSZ 22,W
|
|
|
566 |
012A: GOTO 12C
|
|
|
567 |
012B: GOTO 12F
|
|
|
568 |
012C: MOVF 22,F
|
|
|
569 |
012D: BTFSS 03.2
|
|
|
570 |
012E: GOTO 160
|
|
|
571 |
.................... {
|
|
|
572 |
.................... if (rovinka>250)
|
|
|
573 |
012F: MOVF 24,W
|
|
|
574 |
0130: SUBLW FA
|
|
|
575 |
0131: BTFSC 03.0
|
|
|
576 |
0132: GOTO 15F
|
|
|
577 |
.................... {
|
|
|
578 |
.................... BL; BR;
|
|
|
579 |
0133: BSF 03.5
|
|
|
580 |
0134: BCF 05.0
|
|
|
581 |
0135: BCF 03.5
|
|
|
582 |
0136: BCF 05.0
|
|
|
583 |
0137: BSF 03.5
|
|
|
584 |
0138: BCF 05.1
|
|
|
585 |
0139: BCF 03.5
|
|
|
586 |
013A: BSF 05.1
|
|
|
587 |
013B: BSF 03.5
|
|
|
588 |
013C: BCF 05.6
|
|
|
589 |
013D: BCF 03.5
|
|
|
590 |
013E: BCF 05.6
|
|
|
591 |
013F: BSF 03.5
|
|
|
592 |
0140: BCF 05.7
|
|
|
593 |
0141: BCF 03.5
|
|
|
594 |
0142: BSF 05.7
|
|
|
595 |
.................... for(n=1; n<(60); n++) {rtos_yield(); delay_us(500);};
|
|
|
596 |
0143: CLRF 3F
|
|
|
597 |
0144: MOVLW 01
|
|
|
598 |
0145: MOVWF 3E
|
|
|
599 |
0146: MOVF 3F,F
|
|
|
600 |
0147: BTFSS 03.2
|
|
|
601 |
0148: GOTO 15F
|
|
|
602 |
0149: MOVF 3E,W
|
|
|
603 |
014A: SUBLW 3B
|
|
|
604 |
014B: BTFSS 03.0
|
|
|
605 |
014C: GOTO 15F
|
|
|
606 |
014D: MOVLW 53
|
|
|
607 |
014E: MOVWF 35
|
|
|
608 |
014F: MOVLW 01
|
|
|
609 |
0150: MOVWF 36
|
|
|
610 |
0151: BCF 0A.3
|
|
|
611 |
0152: GOTO 234
|
|
|
612 |
0153: CLRWDT
|
|
|
613 |
0154: MOVLW 02
|
|
|
614 |
0155: MOVWF 41
|
|
|
615 |
0156: MOVLW F7
|
|
|
616 |
0157: MOVWF 42
|
|
|
617 |
0158: CALL 024
|
|
|
618 |
0159: DECFSZ 41,F
|
|
|
619 |
015A: GOTO 156
|
|
|
620 |
015B: INCF 3E,F
|
|
|
621 |
015C: BTFSC 03.2
|
|
|
622 |
015D: INCF 3F,F
|
|
|
623 |
015E: GOTO 146
|
|
|
624 |
.................... };
|
|
|
625 |
.................... rovinka=0;
|
|
|
626 |
015F: CLRF 24
|
|
|
627 |
.................... }
|
|
|
628 |
....................
|
|
|
629 |
.................... if(bit_test(sensors,7)) // zjisti jestli neni cihla
|
|
|
630 |
0160: BTFSS 21.7
|
|
|
631 |
0161: GOTO 163
|
|
|
632 |
.................... {
|
|
|
633 |
.................... rtos_terminate();
|
|
|
634 |
0162: GOTO 241
|
|
|
635 |
.................... }
|
|
|
636 |
.................... }
|
|
|
637 |
0163: MOVLW 51
|
|
|
638 |
0164: MOVWF 35
|
|
|
639 |
0165: MOVLW 00
|
|
|
640 |
0166: MOVWF 36
|
|
|
641 |
0167: BCF 0A.3
|
|
|
642 |
0168: GOTO 234
|
|
|
643 |
0169: RETLW 00
|
|
|
644 |
....................
|
|
|
645 |
....................
|
|
|
646 |
....................
|
|
|
647 |
.................... ////////////////////////////////////////////////////////////////////////////////
|
|
|
648 |
.................... void main()
|
|
|
649 |
.................... {
|
|
|
650 |
*
|
|
|
651 |
0182: CLRF 04
|
|
|
652 |
0183: MOVLW 1F
|
|
|
653 |
0184: ANDWF 03,F
|
|
|
654 |
0185: MOVLW 70
|
|
|
655 |
0186: BSF 03.5
|
|
|
656 |
0187: MOVWF 0F
|
|
|
657 |
0188: BCF 1F.4
|
|
|
658 |
0189: BCF 1F.5
|
|
|
659 |
018A: MOVF 1B,W
|
|
|
660 |
018B: ANDLW 80
|
|
|
661 |
018C: MOVWF 1B
|
|
|
662 |
018D: MOVLW 07
|
|
|
663 |
018E: MOVWF 1C
|
|
|
664 |
.................... unsigned int16 i;
|
|
|
665 |
.................... unsigned int8 last;
|
|
|
666 |
....................
|
|
|
667 |
.................... setup_adc_ports(NO_ANALOGS);
|
|
|
668 |
018F: BCF 1F.4
|
|
|
669 |
0190: BCF 1F.5
|
|
|
670 |
0191: MOVF 1B,W
|
|
|
671 |
0192: ANDLW 80
|
|
|
672 |
0193: MOVWF 1B
|
|
|
673 |
.................... setup_adc(ADC_CLOCK_INTERNAL);
|
|
|
674 |
0194: BCF 1F.6
|
|
|
675 |
0195: BCF 03.5
|
|
|
676 |
0196: BSF 1F.6
|
|
|
677 |
0197: BSF 1F.7
|
|
|
678 |
0198: BSF 03.5
|
|
|
679 |
0199: BCF 1F.7
|
|
|
680 |
019A: BCF 03.5
|
|
|
681 |
019B: BSF 1F.0
|
|
|
682 |
.................... setup_spi(SPI_MASTER|SPI_H_TO_L|SPI_XMIT_L_TO_H|SPI_CLK_DIV_4);
|
|
|
683 |
019C: BCF 14.5
|
|
|
684 |
019D: BSF 03.5
|
|
|
685 |
019E: BCF 06.2
|
|
|
686 |
019F: BSF 06.1
|
|
|
687 |
01A0: BCF 06.4
|
|
|
688 |
01A1: MOVLW 30
|
|
|
689 |
01A2: BCF 03.5
|
|
|
690 |
01A3: MOVWF 14
|
|
|
691 |
01A4: MOVLW 00
|
|
|
692 |
01A5: BSF 03.5
|
|
|
693 |
01A6: MOVWF 14
|
|
|
694 |
.................... setup_timer_0(RTCC_INTERNAL|RTCC_DIV_1);
|
|
|
695 |
01A7: MOVF 01,W
|
|
|
696 |
01A8: ANDLW C7
|
|
|
697 |
01A9: IORLW 08
|
|
|
698 |
01AA: MOVWF 01
|
|
|
699 |
.................... setup_timer_1(T1_DISABLED);
|
|
|
700 |
01AB: BCF 03.5
|
|
|
701 |
01AC: CLRF 10
|
|
|
702 |
.................... setup_oscillator(OSC_8MHZ|OSC_INTRC);
|
|
|
703 |
01AD: MOVLW 72
|
|
|
704 |
01AE: BSF 03.5
|
|
|
705 |
01AF: MOVWF 0F
|
|
|
706 |
....................
|
|
|
707 |
.................... STOPR; STOPL; // zastav motory
|
|
|
708 |
01B0: BCF 05.6
|
|
|
709 |
01B1: BCF 03.5
|
|
|
710 |
01B2: BCF 05.6
|
|
|
711 |
01B3: BSF 03.5
|
|
|
712 |
01B4: BCF 05.7
|
|
|
713 |
01B5: BCF 03.5
|
|
|
714 |
01B6: BCF 05.7
|
|
|
715 |
01B7: BSF 03.5
|
|
|
716 |
01B8: BCF 05.0
|
|
|
717 |
01B9: BCF 03.5
|
|
|
718 |
01BA: BCF 05.0
|
|
|
719 |
01BB: BSF 03.5
|
|
|
720 |
01BC: BCF 05.1
|
|
|
721 |
01BD: BCF 03.5
|
|
|
722 |
01BE: BCF 05.1
|
|
|
723 |
.................... Lmotor=0;Rmotor=0;
|
|
|
724 |
01BF: CLRF 26
|
|
|
725 |
01C0: CLRF 25
|
|
|
726 |
01C1: CLRF 28
|
|
|
727 |
01C2: CLRF 27
|
|
|
728 |
....................
|
|
|
729 |
.................... uhel=STRED; // nastav zadni kolecko na stred
|
|
|
730 |
01C3: MOVLW 80
|
|
|
731 |
01C4: MOVWF 23
|
|
|
732 |
.................... rovinka=0;
|
|
|
733 |
01C5: CLRF 24
|
|
|
734 |
....................
|
|
|
735 |
.................... output_low(IRTX); // zapni IR vysilac
|
|
|
736 |
01C6: BSF 03.5
|
|
|
737 |
01C7: BCF 06.2
|
|
|
738 |
01C8: BCF 03.5
|
|
|
739 |
01C9: BCF 06.2
|
|
|
740 |
....................
|
|
|
741 |
.................... for(i=0;i<100;i++) // pockej, nez se zadni kolecko vystredi
|
|
|
742 |
01CA: CLRF 38
|
|
|
743 |
01CB: CLRF 37
|
|
|
744 |
01CC: MOVF 38,F
|
|
|
745 |
01CD: BTFSS 03.2
|
|
|
746 |
01CE: GOTO 1DB
|
|
|
747 |
01CF: MOVF 37,W
|
|
|
748 |
01D0: SUBLW 63
|
|
|
749 |
01D1: BTFSS 03.0
|
|
|
750 |
01D2: GOTO 1DB
|
|
|
751 |
.................... {
|
|
|
752 |
.................... diag();
|
|
|
753 |
01D3: GOTO 16A
|
|
|
754 |
.................... delay_ms(16);
|
|
|
755 |
01D4: MOVLW 10
|
|
|
756 |
01D5: MOVWF 3D
|
|
|
757 |
01D6: CALL 00C
|
|
|
758 |
.................... }
|
|
|
759 |
01D7: INCF 37,F
|
|
|
760 |
01D8: BTFSC 03.2
|
|
|
761 |
01D9: INCF 38,F
|
|
|
762 |
01DA: GOTO 1CC
|
|
|
763 |
.................... // diagnostika();
|
|
|
764 |
....................
|
|
|
765 |
.................... while(true)
|
|
|
766 |
.................... {
|
|
|
767 |
.................... rtos_run();
|
|
|
768 |
01DB: CLRF 29
|
|
|
769 |
01DC: CLRF 2B
|
|
|
770 |
01DD: MOVLW 09
|
|
|
771 |
01DE: MOVWF 2A
|
|
|
772 |
01DF: CLRF 2D
|
|
|
773 |
01E0: CLRF 2C
|
|
|
774 |
01E1: MOVLW 34
|
|
|
775 |
01E2: MOVWF 2E
|
|
|
776 |
01E3: MOVLW 00
|
|
|
777 |
01E4: MOVWF 2F
|
|
|
778 |
01E5: CLRF 30
|
|
|
779 |
01E6: CLRF 32
|
|
|
780 |
01E7: MOVLW 01
|
|
|
781 |
01E8: MOVWF 31
|
|
|
782 |
01E9: CLRF 34
|
|
|
783 |
01EA: CLRF 33
|
|
|
784 |
01EB: MOVLW 51
|
|
|
785 |
01EC: MOVWF 35
|
|
|
786 |
01ED: MOVLW 00
|
|
|
787 |
01EE: MOVWF 36
|
|
|
788 |
01EF: CLRF 20
|
|
|
789 |
01F0: MOVLW 00
|
|
|
790 |
01F1: MOVWF 78
|
|
|
791 |
01F2: IORLW 06
|
|
|
792 |
01F3: MOVWF 12
|
|
|
793 |
01F4: MOVLW FA
|
|
|
794 |
01F5: BSF 03.5
|
|
|
795 |
01F6: MOVWF 12
|
|
|
796 |
01F7: BCF 03.5
|
|
|
797 |
01F8: BCF 0C.1
|
|
|
798 |
01F9: MOVLW 00
|
|
|
799 |
01FA: MOVWF 78
|
|
|
800 |
01FB: IORLW 06
|
|
|
801 |
01FC: MOVWF 12
|
|
|
802 |
01FD: MOVLW FA
|
|
|
803 |
01FE: BSF 03.5
|
|
|
804 |
01FF: MOVWF 12
|
|
|
805 |
0200: BCF 03.5
|
|
|
806 |
0201: BCF 0C.1
|
|
|
807 |
0202: BCF 03.0
|
|
|
808 |
0203: RLF 20,W
|
|
|
809 |
0204: MOVWF 77
|
|
|
810 |
0205: CALL 004
|
|
|
811 |
0206: MOVWF 79
|
|
|
812 |
0207: MOVLW 01
|
|
|
813 |
0208: ADDWF 77,W
|
|
|
814 |
0209: CALL 004
|
|
|
815 |
020A: MOVWF 7A
|
|
|
816 |
020B: MOVF 79,W
|
|
|
817 |
020C: MOVWF 04
|
|
|
818 |
020D: MOVF 00,W
|
|
|
819 |
020E: MOVWF 79
|
|
|
820 |
020F: INCF 04,F
|
|
|
821 |
0210: MOVF 00,W
|
|
|
822 |
0211: MOVWF 77
|
|
|
823 |
0212: INCF 04,F
|
|
|
824 |
0213: MOVF 00,W
|
|
|
825 |
0214: MOVWF 78
|
|
|
826 |
0215: INCF 04,F
|
|
|
827 |
0216: INCF 00,F
|
|
|
828 |
0217: MOVF 00,W
|
|
|
829 |
0218: MOVWF 7A
|
|
|
830 |
0219: BTFSS 03.2
|
|
|
831 |
021A: GOTO 21E
|
|
|
832 |
021B: INCF 04,F
|
|
|
833 |
021C: INCF 00,F
|
|
|
834 |
021D: DECF 04,F
|
|
|
835 |
021E: INCF 04,F
|
|
|
836 |
021F: MOVF 00,W
|
|
|
837 |
0220: SUBWF 78,W
|
|
|
838 |
0221: BTFSS 03.2
|
|
|
839 |
0222: GOTO 236
|
|
|
840 |
0223: MOVF 77,W
|
|
|
841 |
0224: SUBWF 7A,W
|
|
|
842 |
0225: BTFSS 03.2
|
|
|
843 |
0226: GOTO 236
|
|
|
844 |
0227: DECF 04,F
|
|
|
845 |
0228: CLRF 00
|
|
|
846 |
0229: INCF 04,F
|
|
|
847 |
022A: CLRF 00
|
|
|
848 |
022B: INCF 04,F
|
|
|
849 |
022C: BTFSC 79.7
|
|
|
850 |
022D: GOTO 236
|
|
|
851 |
022E: INCF 04,F
|
|
|
852 |
022F: MOVF 00,W
|
|
|
853 |
0230: MOVWF 0A
|
|
|
854 |
0231: DECF 04,F
|
|
|
855 |
0232: MOVF 00,W
|
|
|
856 |
0233: MOVWF 02
|
|
|
857 |
0234: BCF 03.5
|
|
|
858 |
0235: BCF 03.6
|
|
|
859 |
0236: INCF 20,F
|
|
|
860 |
0237: MOVLW 02
|
|
|
861 |
0238: SUBWF 20,W
|
|
|
862 |
0239: BTFSS 03.2
|
|
|
863 |
023A: GOTO 202
|
|
|
864 |
023B: CLRF 20
|
|
|
865 |
023C: BTFSS 0C.1
|
|
|
866 |
023D: GOTO 240
|
|
|
867 |
023E: BSF 03.5
|
|
|
868 |
023F: GOTO 200
|
|
|
869 |
0240: GOTO 23C
|
|
|
870 |
0241: MOVLW FF
|
|
|
871 |
0242: MOVWF 20
|
|
|
872 |
.................... STOPR;
|
|
|
873 |
0243: BSF 03.5
|
|
|
874 |
0244: BCF 05.6
|
|
|
875 |
0245: BCF 03.5
|
|
|
876 |
0246: BCF 05.6
|
|
|
877 |
0247: BSF 03.5
|
|
|
878 |
0248: BCF 05.7
|
|
|
879 |
0249: BCF 03.5
|
|
|
880 |
024A: BCF 05.7
|
|
|
881 |
.................... STOPL;
|
|
|
882 |
024B: BSF 03.5
|
|
|
883 |
024C: BCF 05.0
|
|
|
884 |
024D: BCF 03.5
|
|
|
885 |
024E: BCF 05.0
|
|
|
886 |
024F: BSF 03.5
|
|
|
887 |
0250: BCF 05.1
|
|
|
888 |
0251: BCF 03.5
|
|
|
889 |
0252: BCF 05.1
|
|
|
890 |
.................... while(true);
|
|
|
891 |
0253: GOTO 253
|
|
|
892 |
.................... }
|
|
|
893 |
0254: GOTO 1DB
|
|
|
894 |
.................... }
|
|
|
895 |
0255: SLEEP
|
|
|
896 |
|
|
|
897 |
Configuration Fuses:
|
|
|
898 |
Word 1: 3F38 NOWDT NOPUT MCLR NOBROWNOUT NOLVP NOCPD NOWRT NODEBUG CCPB0 NOPROTECT INTRC_IO
|
|
|
899 |
Word 2: 3FFC NOFCMEN NOIESO
|